diff options
author | Campbell Barton <ideasman42@gmail.com> | 2009-03-26 23:17:05 +0300 |
---|---|---|
committer | Campbell Barton <ideasman42@gmail.com> | 2009-03-26 23:17:05 +0300 |
commit | 2c585ae28ef7cdba01894144076ad3fa60ef1ff9 (patch) | |
tree | f44b8d5d0d152b08ee2ffe2e4221a110abc120c0 /extern/ode/dist | |
parent | b68aceda2f070207f65e2068d11ac9296a7c4b40 (diff) |
remove ODE, build systems and BGE are ignoring it already.
Diffstat (limited to 'extern/ode/dist')
97 files changed, 0 insertions, 21849 deletions
diff --git a/extern/ode/dist/INSTALL b/extern/ode/dist/INSTALL deleted file mode 100644 index f82285db1b2..00000000000 --- a/extern/ode/dist/INSTALL +++ /dev/null @@ -1,44 +0,0 @@ - -here are the steps to buid ODE: - -(1) get the GNU 'make' tool. many unix platforms come with this, although - sometimes it is called 'gmake'. i have provided a version of GNU make - for windows at: http://q12.org/ode/bin/make.exe - -(2) edit the settings in the file config/user-settings. the list of supported - platforms is given in that file. - -(3) run 'make' to configure and build ODE and the graphical test programs. - to build parts of ODE the make targets are: - - make configure create configuration file include/ode/config.h - make ode-lib build the core ODE library - make drawstuff-lib build the OpenGL-based graphics library - make ode-test build some ODE tests (they need drawstuff) - make drawstuff-test build a test app for the drawstuff library - - all of these targets will do an implicit 'make configure'. if the - configurator screws up then you can edit the settings directly in - include/ode/config.h. - -(4) to install the ODE library onto your system you should copy the 'lib' and - 'include' directories to a suitable place, e.g. on unix: - - include/ode --> /usr/local/include/ode - lib/libode.a --> /usr/local/lib/libode.a - -ODE has been verified to build on the following platforms: - - config ode-lib ode-test - ------ ------- -------- - windows - MSVC msvc * * - MinGW mingw * * - CygWin cygwin * * - linux (x86, mandrake 8.1) unix-gcc * * - linux (alpha, debian 2.2) unix-gcc * ? - linux (RS/6000, debian 2.2) unix-gcc * ? - linux (Sparc U60, debian 2.2) unix-gcc * ? - freebsd 4.3 unix-gcc * ? - Mac OS-X osx * ? - Solaris 8 (Sparc R220) unix-gcc * ? diff --git a/extern/ode/dist/LICENSE-BSD.TXT b/extern/ode/dist/LICENSE-BSD.TXT deleted file mode 100644 index 05929239487..00000000000 --- a/extern/ode/dist/LICENSE-BSD.TXT +++ /dev/null @@ -1,34 +0,0 @@ - -This is the BSD-style license for the Open Dynamics Engine ----------------------------------------------------------- - -Open Dynamics Engine -Copyright (c) 2001,2002, Russell L. Smith. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions -are met: - -Redistributions of source code must retain the above copyright notice, -this list of conditions and the following disclaimer. - -Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the following disclaimer in the documentation -and/or other materials provided with the distribution. - -Neither the names of ODE's copyright owner nor the names of its -contributors may be used to endorse or promote products derived from -this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR -PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF -LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING -NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/extern/ode/dist/LICENSE.TXT b/extern/ode/dist/LICENSE.TXT deleted file mode 100644 index cfe59bcadb8..00000000000 --- a/extern/ode/dist/LICENSE.TXT +++ /dev/null @@ -1,502 +0,0 @@ - GNU LESSER GENERAL PUBLIC LICENSE - Version 2.1, February 1999 - - Copyright (C) 1991, 1999 Free Software Foundation, Inc. - 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - -[This is the first released version of the Lesser GPL. It also counts - as the successor of the GNU Library Public License, version 2, hence - the version number 2.1.] - - Preamble - - The licenses for most software are designed to take away your -freedom to share and change it. By contrast, the GNU General Public -Licenses are intended to guarantee your freedom to share and change -free software--to make sure the software is free for all its users. - - This license, the Lesser General Public License, applies to some -specially designated software packages--typically libraries--of the -Free Software Foundation and other authors who decide to use it. You -can use it too, but we suggest you first think carefully about whether -this license or the ordinary General Public License is the better -strategy to use in any particular case, based on the explanations below. - - When we speak of free software, we are referring to freedom of use, -not price. Our General Public Licenses are designed to make sure that -you have the freedom to distribute copies of free software (and charge -for this service if you wish); that you receive source code or can get -it if you want it; that you can change the software and use pieces of -it in new free programs; and that you are informed that you can do -these things. - - To protect your rights, we need to make restrictions that forbid -distributors to deny you these rights or to ask you to surrender these -rights. These restrictions translate to certain responsibilities for -you if you distribute copies of the library or if you modify it. - - For example, if you distribute copies of the library, whether gratis -or for a fee, you must give the recipients all the rights that we gave -you. You must make sure that they, too, receive or can get the source -code. If you link other code with the library, you must provide -complete object files to the recipients, so that they can relink them -with the library after making changes to the library and recompiling -it. And you must show them these terms so they know their rights. - - We protect your rights with a two-step method: (1) we copyright the -library, and (2) we offer you this license, which gives you legal -permission to copy, distribute and/or modify the library. - - To protect each distributor, we want to make it very clear that -there is no warranty for the free library. Also, if the library is -modified by someone else and passed on, the recipients should know -that what they have is not the original version, so that the original -author's reputation will not be affected by problems that might be -introduced by others. - - Finally, software patents pose a constant threat to the existence of -any free program. We wish to make sure that a company cannot -effectively restrict the users of a free program by obtaining a -restrictive license from a patent holder. Therefore, we insist that -any patent license obtained for a version of the library must be -consistent with the full freedom of use specified in this license. - - Most GNU software, including some libraries, is covered by the -ordinary GNU General Public License. This license, the GNU Lesser -General Public License, applies to certain designated libraries, and -is quite different from the ordinary General Public License. We use -this license for certain libraries in order to permit linking those -libraries into non-free programs. - - When a program is linked with a library, whether statically or using -a shared library, the combination of the two is legally speaking a -combined work, a derivative of the original library. The ordinary -General Public License therefore permits such linking only if the -entire combination fits its criteria of freedom. The Lesser General -Public License permits more lax criteria for linking other code with -the library. - - We call this license the "Lesser" General Public License because it -does Less to protect the user's freedom than the ordinary General -Public License. It also provides other free software developers Less -of an advantage over competing non-free programs. These disadvantages -are the reason we use the ordinary General Public License for many -libraries. However, the Lesser license provides advantages in certain -special circumstances. - - For example, on rare occasions, there may be a special need to -encourage the widest possible use of a certain library, so that it becomes -a de-facto standard. To achieve this, non-free programs must be -allowed to use the library. A more frequent case is that a free -library does the same job as widely used non-free libraries. In this -case, there is little to gain by limiting the free library to free -software only, so we use the Lesser General Public License. - - In other cases, permission to use a particular library in non-free -programs enables a greater number of people to use a large body of -free software. For example, permission to use the GNU C Library in -non-free programs enables many more people to use the whole GNU -operating system, as well as its variant, the GNU/Linux operating -system. - - Although the Lesser General Public License is Less protective of the -users' freedom, it does ensure that the user of a program that is -linked with the Library has the freedom and the wherewithal to run -that program using a modified version of the Library. - - The precise terms and conditions for copying, distribution and -modification follow. Pay close attention to the difference between a -"work based on the library" and a "work that uses the library". The -former contains code derived from the library, whereas the latter must -be combined with the library in order to run. - - GNU LESSER GENERAL PUBLIC LICENSE - TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION - - 0. This License Agreement applies to any software library or other -program which contains a notice placed by the copyright holder or -other authorized party saying it may be distributed under the terms of -this Lesser General Public License (also called "this License"). -Each licensee is addressed as "you". - - A "library" means a collection of software functions and/or data -prepared so as to be conveniently linked with application programs -(which use some of those functions and data) to form executables. - - The "Library", below, refers to any such software library or work -which has been distributed under these terms. A "work based on the -Library" means either the Library or any derivative work under -copyright law: that is to say, a work containing the Library or a -portion of it, either verbatim or with modifications and/or translated -straightforwardly into another language. (Hereinafter, translation is -included without limitation in the term "modification".) - - "Source code" for a work means the preferred form of the work for -making modifications to it. For a library, complete source code means -all the source code for all modules it contains, plus any associated -interface definition files, plus the scripts used to control compilation -and installation of the library. - - Activities other than copying, distribution and modification are not -covered by this License; they are outside its scope. The act of -running a program using the Library is not restricted, and output from -such a program is covered only if its contents constitute a work based -on the Library (independent of the use of the Library in a tool for -writing it). Whether that is true depends on what the Library does -and what the program that uses the Library does. - - 1. You may copy and distribute verbatim copies of the Library's -complete source code as you receive it, in any medium, provided that -you conspicuously and appropriately publish on each copy an -appropriate copyright notice and disclaimer of warranty; keep intact -all the notices that refer to this License and to the absence of any -warranty; and distribute a copy of this License along with the -Library. - - You may charge a fee for the physical act of transferring a copy, -and you may at your option offer warranty protection in exchange for a -fee. - - 2. You may modify your copy or copies of the Library or any portion -of it, thus forming a work based on the Library, and copy and -distribute such modifications or work under the terms of Section 1 -above, provided that you also meet all of these conditions: - - a) The modified work must itself be a software library. - - b) You must cause the files modified to carry prominent notices - stating that you changed the files and the date of any change. - - c) You must cause the whole of the work to be licensed at no - charge to all third parties under the terms of this License. - - d) If a facility in the modified Library refers to a function or a - table of data to be supplied by an application program that uses - the facility, other than as an argument passed when the facility - is invoked, then you must make a good faith effort to ensure that, - in the event an application does not supply such function or - table, the facility still operates, and performs whatever part of - its purpose remains meaningful. - - (For example, a function in a library to compute square roots has - a purpose that is entirely well-defined independent of the - application. Therefore, Subsection 2d requires that any - application-supplied function or table used by this function must - be optional: if the application does not supply it, the square - root function must still compute square roots.) - -These requirements apply to the modified work as a whole. If -identifiable sections of that work are not derived from the Library, -and can be reasonably considered independent and separate works in -themselves, then this License, and its terms, do not apply to those -sections when you distribute them as separate works. But when you -distribute the same sections as part of a whole which is a work based -on the Library, the distribution of the whole must be on the terms of -this License, whose permissions for other licensees extend to the -entire whole, and thus to each and every part regardless of who wrote -it. - -Thus, it is not the intent of this section to claim rights or contest -your rights to work written entirely by you; rather, the intent is to -exercise the right to control the distribution of derivative or -collective works based on the Library. - -In addition, mere aggregation of another work not based on the Library -with the Library (or with a work based on the Library) on a volume of -a storage or distribution medium does not bring the other work under -the scope of this License. - - 3. You may opt to apply the terms of the ordinary GNU General Public -License instead of this License to a given copy of the Library. To do -this, you must alter all the notices that refer to this License, so -that they refer to the ordinary GNU General Public License, version 2, -instead of to this License. (If a newer version than version 2 of the -ordinary GNU General Public License has appeared, then you can specify -that version instead if you wish.) Do not make any other change in -these notices. - - Once this change is made in a given copy, it is irreversible for -that copy, so the ordinary GNU General Public License applies to all -subsequent copies and derivative works made from that copy. - - This option is useful when you wish to copy part of the code of -the Library into a program that is not a library. - - 4. You may copy and distribute the Library (or a portion or -derivative of it, under Section 2) in object code or executable form -under the terms of Sections 1 and 2 above provided that you accompany -it with the complete corresponding machine-readable source code, which -must be distributed under the terms of Sections 1 and 2 above on a -medium customarily used for software interchange. - - If distribution of object code is made by offering access to copy -from a designated place, then offering equivalent access to copy the -source code from the same place satisfies the requirement to -distribute the source code, even though third parties are not -compelled to copy the source along with the object code. - - 5. A program that contains no derivative of any portion of the -Library, but is designed to work with the Library by being compiled or -linked with it, is called a "work that uses the Library". Such a -work, in isolation, is not a derivative work of the Library, and -therefore falls outside the scope of this License. - - However, linking a "work that uses the Library" with the Library -creates an executable that is a derivative of the Library (because it -contains portions of the Library), rather than a "work that uses the -library". The executable is therefore covered by this License. -Section 6 states terms for distribution of such executables. - - When a "work that uses the Library" uses material from a header file -that is part of the Library, the object code for the work may be a -derivative work of the Library even though the source code is not. -Whether this is true is especially significant if the work can be -linked without the Library, or if the work is itself a library. The -threshold for this to be true is not precisely defined by law. - - If such an object file uses only numerical parameters, data -structure layouts and accessors, and small macros and small inline -functions (ten lines or less in length), then the use of the object -file is unrestricted, regardless of whether it is legally a derivative -work. (Executables containing this object code plus portions of the -Library will still fall under Section 6.) - - Otherwise, if the work is a derivative of the Library, you may -distribute the object code for the work under the terms of Section 6. -Any executables containing that work also fall under Section 6, -whether or not they are linked directly with the Library itself. - - 6. As an exception to the Sections above, you may also combine or -link a "work that uses the Library" with the Library to produce a -work containing portions of the Library, and distribute that work -under terms of your choice, provided that the terms permit -modification of the work for the customer's own use and reverse -engineering for debugging such modifications. - - You must give prominent notice with each copy of the work that the -Library is used in it and that the Library and its use are covered by -this License. You must supply a copy of this License. If the work -during execution displays copyright notices, you must include the -copyright notice for the Library among them, as well as a reference -directing the user to the copy of this License. Also, you must do one -of these things: - - a) Accompany the work with the complete corresponding - machine-readable source code for the Library including whatever - changes were used in the work (which must be distributed under - Sections 1 and 2 above); and, if the work is an executable linked - with the Library, with the complete machine-readable "work that - uses the Library", as object code and/or source code, so that the - user can modify the Library and then relink to produce a modified - executable containing the modified Library. (It is understood - that the user who changes the contents of definitions files in the - Library will not necessarily be able to recompile the application - to use the modified definitions.) - - b) Use a suitable shared library mechanism for linking with the - Library. A suitable mechanism is one that (1) uses at run time a - copy of the library already present on the user's computer system, - rather than copying library functions into the executable, and (2) - will operate properly with a modified version of the library, if - the user installs one, as long as the modified version is - interface-compatible with the version that the work was made with. - - c) Accompany the work with a written offer, valid for at - least three years, to give the same user the materials - specified in Subsection 6a, above, for a charge no more - than the cost of performing this distribution. - - d) If distribution of the work is made by offering access to copy - from a designated place, offer equivalent access to copy the above - specified materials from the same place. - - e) Verify that the user has already received a copy of these - materials or that you have already sent this user a copy. - - For an executable, the required form of the "work that uses the -Library" must include any data and utility programs needed for -reproducing the executable from it. However, as a special exception, -the materials to be distributed need not include anything that is -normally distributed (in either source or binary form) with the major -components (compiler, kernel, and so on) of the operating system on -which the executable runs, unless that component itself accompanies -the executable. - - It may happen that this requirement contradicts the license -restrictions of other proprietary libraries that do not normally -accompany the operating system. Such a contradiction means you cannot -use both them and the Library together in an executable that you -distribute. - - 7. You may place library facilities that are a work based on the -Library side-by-side in a single library together with other library -facilities not covered by this License, and distribute such a combined -library, provided that the separate distribution of the work based on -the Library and of the other library facilities is otherwise -permitted, and provided that you do these two things: - - a) Accompany the combined library with a copy of the same work - based on the Library, uncombined with any other library - facilities. This must be distributed under the terms of the - Sections above. - - b) Give prominent notice with the combined library of the fact - that part of it is a work based on the Library, and explaining - where to find the accompanying uncombined form of the same work. - - 8. You may not copy, modify, sublicense, link with, or distribute -the Library except as expressly provided under this License. Any -attempt otherwise to copy, modify, sublicense, link with, or -distribute the Library is void, and will automatically terminate your -rights under this License. However, parties who have received copies, -or rights, from you under this License will not have their licenses -terminated so long as such parties remain in full compliance. - - 9. You are not required to accept this License, since you have not -signed it. However, nothing else grants you permission to modify or -distribute the Library or its derivative works. These actions are -prohibited by law if you do not accept this License. Therefore, by -modifying or distributing the Library (or any work based on the -Library), you indicate your acceptance of this License to do so, and -all its terms and conditions for copying, distributing or modifying -the Library or works based on it. - - 10. Each time you redistribute the Library (or any work based on the -Library), the recipient automatically receives a license from the -original licensor to copy, distribute, link with or modify the Library -subject to these terms and conditions. You may not impose any further -restrictions on the recipients' exercise of the rights granted herein. -You are not responsible for enforcing compliance by third parties with -this License. - - 11. If, as a consequence of a court judgment or allegation of patent -infringement or for any other reason (not limited to patent issues), -conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot -distribute so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you -may not distribute the Library at all. For example, if a patent -license would not permit royalty-free redistribution of the Library by -all those who receive copies directly or indirectly through you, then -the only way you could satisfy both it and this License would be to -refrain entirely from distribution of the Library. - -If any portion of this section is held invalid or unenforceable under any -particular circumstance, the balance of the section is intended to apply, -and the section as a whole is intended to apply in other circumstances. - -It is not the purpose of this section to induce you to infringe any -patents or other property right claims or to contest validity of any -such claims; this section has the sole purpose of protecting the -integrity of the free software distribution system which is -implemented by public license practices. Many people have made -generous contributions to the wide range of software distributed -through that system in reliance on consistent application of that -system; it is up to the author/donor to decide if he or she is willing -to distribute software through any other system and a licensee cannot -impose that choice. - -This section is intended to make thoroughly clear what is believed to -be a consequence of the rest of this License. - - 12. If the distribution and/or use of the Library is restricted in -certain countries either by patents or by copyrighted interfaces, the -original copyright holder who places the Library under this License may add -an explicit geographical distribution limitation excluding those countries, -so that distribution is permitted only in or among countries not thus -excluded. In such case, this License incorporates the limitation as if -written in the body of this License. - - 13. The Free Software Foundation may publish revised and/or new -versions of the Lesser General Public License from time to time. -Such new versions will be similar in spirit to the present version, -but may differ in detail to address new problems or concerns. - -Each version is given a distinguishing version number. If the Library -specifies a version number of this License which applies to it and -"any later version", you have the option of following the terms and -conditions either of that version or of any later version published by -the Free Software Foundation. If the Library does not specify a -license version number, you may choose any version ever published by -the Free Software Foundation. - - 14. If you wish to incorporate parts of the Library into other free -programs whose distribution conditions are incompatible with these, -write to the author to ask for permission. For software which is -copyrighted by the Free Software Foundation, write to the Free -Software Foundation; we sometimes make exceptions for this. Our -decision will be guided by the two goals of preserving the free status -of all derivatives of our free software and of promoting the sharing -and reuse of software generally. - - NO WARRANTY - - 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO -WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. -EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR -OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY -KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE -LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME -THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. - - 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN -WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY -AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU -FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR -CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE -LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING -RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A -FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF -SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH -DAMAGES. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Libraries - - If you develop a new library, and you want it to be of the greatest -possible use to the public, we recommend making it free software that -everyone can redistribute and change. You can do so by permitting -redistribution under these terms (or, alternatively, under the terms of the -ordinary General Public License). - - To apply these terms, attach the following notices to the library. It is -safest to attach them to the start of each source file to most effectively -convey the exclusion of warranty; and each file should have at least the -"copyright" line and a pointer to where the full notice is found. - - <one line to give the library's name and a brief idea of what it does.> - Copyright (C) <year> <name of author> - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - -Also add information on how to contact you by electronic and paper mail. - -You should also get your employer (if you work as a programmer) or your -school, if any, to sign a "copyright disclaimer" for the library, if -necessary. Here is a sample; alter the names: - - Yoyodyne, Inc., hereby disclaims all copyright interest in the - library `Frob' (a library for tweaking knobs) written by James Random Hacker. - - <signature of Ty Coon>, 1 April 1990 - Ty Coon, President of Vice - -That's all there is to it! diff --git a/extern/ode/dist/Makefile b/extern/ode/dist/Makefile deleted file mode 100644 index 34fbd53f792..00000000000 --- a/extern/ode/dist/Makefile +++ /dev/null @@ -1,280 +0,0 @@ -######################################################################### -# # -# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. # -# All rights reserved. Email: russ@q12.org Web: www.q12.org # -# # -# This library is free software; you can redistribute it and/or # -# modify it under the terms of EITHER: # -# (1) The GNU Lesser General Public License as published by the Free # -# Software Foundation; either version 2.1 of the License, or (at # -# your option) any later version. The text of the GNU Lesser # -# General Public License is included with this library in the # -# file LICENSE.TXT. # -# (2) The BSD-style license that is included with this library in # -# the file LICENSE-BSD.TXT. # -# # -# This library is distributed in the hope that it will be useful, # -# but WITHOUT ANY WARRANTY; without even the implied warranty of # -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files # -# LICENSE.TXT and LICENSE-BSD.TXT for more details. # -# # -######################################################################### - -USER_SETTINGS=config/user-settings -include $(USER_SETTINGS) -PLATFORM_MAKEFILE=config/makefile.$(PLATFORM) -include $(PLATFORM_MAKEFILE) - -############################################################################## -# check some variables that were supposed to be defined - -ifneq ($(BUILD),debug) -ifneq ($(BUILD),release) -$(error the BUILD variable is not set properly) -endif -endif - -ifneq ($(PRECISION),SINGLE) -ifneq ($(PRECISION),DOUBLE) -$(error the PRECISION variable is not set properly) -endif -endif - -############################################################################## -# package settings - -ODE_SRC = \ - ode/src/array.cpp \ - ode/src/error.cpp \ - ode/src/memory.cpp \ - ode/src/obstack.cpp \ - ode/src/odemath.cpp \ - ode/src/matrix.cpp \ - ode/src/misc.cpp \ - ode/src/rotation.cpp \ - ode/src/mass.cpp \ - ode/src/ode.cpp \ - ode/src/step.cpp \ - ode/src/lcp.cpp \ - ode/src/joint.cpp \ - ode/src/space.cpp \ - ode/src/geom.cpp \ - ode/src/timer.cpp \ - ode/src/mat.cpp \ - ode/src/testing.cpp -ODE_PREGEN_SRC = \ - ode/src/fastldlt.c \ - ode/src/fastlsolve.c \ - ode/src/fastltsolve.c \ - ode/src/fastdot.c - -ifeq ($(WINDOWS),1) -DRAWSTUFF_SRC = drawstuff/src/drawstuff.cpp drawstuff/src/windows.cpp -RESOURCE_FILE=lib/resources.RES -else -DRAWSTUFF_SRC = drawstuff/src/drawstuff.cpp drawstuff/src/x11.cpp -endif - -ODE_LIB_NAME=ode -DRAWSTUFF_LIB_NAME=drawstuff - -INCPATH=include -LIBPATH=lib - -ODE_TEST_SRC_CPP = \ - ode/test/test_ode.cpp \ - ode/test/test_chain2.cpp \ - ode/test/test_hinge.cpp \ - ode/test/test_slider.cpp \ - ode/test/test_collision.cpp \ - ode/test/test_boxstack.cpp \ - ode/test/test_buggy.cpp \ - ode/test/test_joints.cpp \ - ode/test/test_space.cpp \ - ode/test/test_I.cpp \ - ode/test/test_step.cpp \ - ode/test/test_friction.cpp -ODE_TEST_SRC_C = \ - ode/test/test_chain1.c -DRAWSTUFF_TEST_SRC_CPP = \ - drawstuff/dstest/dstest.cpp - -CONFIGURATOR_SRC=configurator.c -CONFIG_H=include/ode/config.h - -############################################################################## -# derived things - -DEFINES= - -# add some defines depending on the build mode -ifeq ($(BUILD),release) -DEFINES+=$(C_DEF)dNODEBUG -endif -ifeq ($(BUILD),debug) -DEFINES+=$(C_DEF)dDEBUG_ALLOC -endif - -# object file names -ODE_PREGEN_OBJECTS=$(ODE_PREGEN_SRC:%.c=%$(OBJ)) -ODE_OBJECTS=$(ODE_SRC:%.cpp=%$(OBJ)) $(ODE_PREGEN_OBJECTS) -DRAWSTUFF_OBJECTS=$(DRAWSTUFF_SRC:%.cpp=%$(OBJ)) $(RESOURCE_FILE) - -# side-effect variables causing creation of files containing lists of -# filenames to be linked, to work around command-line-length limitations -# on outdated 16-bit operating systems. because of command-line length -# limitations we cannot issue a link command with all object filenames -# specified (because this command is too long and overflows the command -# buffer), but instead must create a file containing all object filenames -# to be linked, and specify this list-file with @listfile on the command-line. -# -# the difficult part is doing this in a flexible way; we don't want to -# hard-code the to-be-linked object filenames in a file, but instead -# want to dynamically create a file containing a list of all object filenames -# within the $XXX_OBJECTS makefile variables. to do this, we use side-effect -# variables. -# -# idea: when these variables are EVALUATED (i.e. later during rule execution, -# not now during variable definition), they cause a SIDE EFFECT which creates -# a file with the list of all ODE object files. why the chicanery??? because -# if we have a command-line length limitation, no SINGLE command we issue will -# be able to create a file containing all object files to be linked -# (because that command itself would need to include all filenames, making -# it too long to be executed). instead, we must use the gnu-make "foreach" -# function, combined - probably in an unintended way - with the "shell" -# function. this is probably unintended because we are not using the "shell" -# function to return a string value for variable evaluation, but are instead -# using the "shell" function to cause a side effect (appending of each filename -# to the filename-list-file). -# -# one possible snag is that, forbidding use of any external EXE utilities and -# relying only on the facilities provided by the outdated 16-bit operating -# system, there is no way to issue a SERIES of commands which append text to -# the end of a file WITHOUT adding newlines. therefore, the list of to-be- -# linked object files is separated by newlines in the list file. fortunately, -# the linker utility for this outdated 16-bit operating system accepts -# filenames on separate lines in the list file. - -# remember: when we evaluate these variables later, this causes the creation -# of the appropriate list file. -ifeq ($(WINDOWS16),1) -SIDE_EFFECT_ODE_OBJLIST = $(foreach o,$(ODE_OBJECTS),$(shell echo $(o) >> odeobj.txt )) -SIDE_EFFECT_DRAWSTUFF_OBJLIST = $(foreach o,$(DRAWSTUFF_OBJECTS),$(shell echo $(o) >> dsobj.txt )) -endif - -# library file names -ODE_LIB=$(LIBPATH)/$(LIB_PREFIX)$(ODE_LIB_NAME)$(LIB_SUFFIX) -DRAWSTUFF_LIB=$(LIBPATH)/$(LIB_PREFIX)$(DRAWSTUFF_LIB_NAME)$(LIB_SUFFIX) - -# executable file names -ODE_TEST_EXE=$(ODE_TEST_SRC_CPP:%.cpp=%.exe) $(ODE_TEST_SRC_C:%.c=%.exe) -DRAWSTUFF_TEST_EXE=$(DRAWSTUFF_TEST_SRC_CPP:%.cpp=%.exe) -CONFIGURATOR_EXE=$(CONFIGURATOR_SRC:%.c=%.exe) - -############################################################################## -# rules -# -# NOTE: the '.c' files are pregenerated sources, and must be compiled with -# -O1 optimization. that is why the rule for .c files is a bit different. -# why should it be compiled with O1? it is numerical code that is generated -# by fbuild. O1 optimization is used to preserve the operation orders that -# were discovered by fbuild to be the fastest on that platform. believe it or -# not, O2 makes this code run much slower for most compilers. - -debug: all -all: ode-lib drawstuff-lib ode-test drawstuff-test - @echo SUCCESS - -ode-lib: configure $(ODE_LIB) -drawstuff-lib: configure $(DRAWSTUFF_LIB) -ode-test: ode-lib drawstuff-lib $(ODE_TEST_EXE) -drawstuff-test: drawstuff-lib $(DRAWSTUFF_TEST_EXE) - -ifndef ODE_LIB_AR_RULE -ODE_LIB_AR_RULE=$(AR)$@ -endif - -$(ODE_LIB): pre_ode_lib $(ODE_OBJECTS) -ifeq ($(WINDOWS16),1) -# if we have a command-line-length limitation, then dynamically create -# a file containing all object filenames, and pass this file to the linker -# instead of directly specifying the object filenames on the command line. -# the very evaluation of the following variable causes creation of file -# odeobj.txt - $(SIDE_EFFECT_ODE_OBJLIST) - $(ODE_LIB_AR_RULE) @odeobj.txt -else -# if we have no command-line-length limitation, directly specify all -# object files to be linked. - $(ODE_LIB_AR_RULE) $(ODE_OBJECTS) -endif - -ifdef RANLIB - $(RANLIB) $@ -endif - -$(DRAWSTUFF_LIB): pre_drawstuff_lib $(DRAWSTUFF_OBJECTS) -ifeq ($WINDOWS16),1) -# if we have a command-line-length limitation, then do the same as above. - $(SIDE_EFFECT_DRAWSTUFF_OBJLIST) - $(AR)$@ @dsobj.txt -else -# if we have no command-line-length limitation, directly specify all object -# files to be linked. - $(AR)$@ $(DRAWSTUFF_OBJECTS) -endif -ifdef RANLIB - $(RANLIB) $@ -endif - -# rules to be executed before library linking starts: delete list file (if one is used) - -pre_ode_lib: -ifeq ($WINDOWS16),1) - $(DEL_CMD) odeobj.txt -endif - -pre_drawstuff_lib: -ifeq ($WINDOWS16),1) - $(DEL_CMD) dsobj.txt -endif - -clean: - -$(DEL_CMD) $(ODE_OBJECTS) $(ODE_TEST_EXE) $(ODE_LIB) $(DRAWSTUFF_OBJECTS) $(DRAWSTUFF_TEST_EXE) $(DRAWSTUFF_LIB) ode/test/*$(OBJ) drawstuff/dstest/*$(OBJ) $(CONFIGURATOR_EXE) $(CONFIG_H) - -%$(OBJ): %.c - $(CC) $(C_FLAGS) $(C_INC)$(INCPATH) $(DEFINES) $(C_OPT)1 $(C_OUT)$@ $< - -%$(OBJ): %.cpp - $(CC) $(C_FLAGS) $(C_INC)$(INCPATH) $(DEFINES) $(C_OPT)$(OPT) $(C_OUT)$@ $< - -%.exe: %$(OBJ) - $(CC) $(C_EXEOUT)$@ $< $(ODE_LIB) $(DRAWSTUFF_LIB) $(RESOURCE_FILE) $(LINK_OPENGL) $(LINK_MATH) - -# windows specific rules - -lib/resources.RES: drawstuff/src/resources.rc - $(RC_RULE) - - -# configurator rules - -configure: $(CONFIG_H) - -$(CONFIG_H): $(CONFIGURATOR_EXE) $(USER_SETTINGS) $(PLATFORM_MAKEFILE) - $(THIS_DIR)$(CONFIGURATOR_EXE) $(CONFIG_H) "$(CC) $(DEFINES) $(C_EXEOUT)" "$(DEL_CMD)" $(THIS_DIR) - -$(CONFIGURATOR_EXE): $(CONFIGURATOR_SRC) $(USER_SETTINGS) $(PLATFORM_MAKEFILE) - $(CC) $(C_DEF)d$(PRECISION) $(DEFINES) $(C_EXEOUT)$@ $< - - -# unix-gcc specific dependency making - -DEP_RULE=gcc -M $(C_INC)$(INCPATH) $(DEFINES) -depend: $(ODE_SRC) $(ODE_PREGEN_SRC) $(DRAWSTUFF_SRC) $(ODE_TEST_SRC_CPP) $(ODE_TEST_SRC_C) $(DRAWSTUFF_TEST_SRC_CPP) - $(DEP_RULE) $(ODE_SRC) $(ODE_PREGEN_SRC) | tools/process_deps ode/src/ > Makefile.deps - $(DEP_RULE) $(DRAWSTUFF_SRC) | tools/process_deps drawstuff/src/ >> Makefile.deps - $(DEP_RULE) $(ODE_TEST_SRC_CPP) | tools/process_deps ode/test/ >> Makefile.deps - $(DEP_RULE) $(DRAWSTUFF_TEST_SRC_CPP) | tools/process_deps drawstuff/dstest/ >> Makefile.deps - -include Makefile.deps diff --git a/extern/ode/dist/Makefile.deps b/extern/ode/dist/Makefile.deps deleted file mode 100644 index ad11f01353f..00000000000 --- a/extern/ode/dist/Makefile.deps +++ /dev/null @@ -1,456 +0,0 @@ -ode/src/array.o: \ - ode/src/array.cpp \ - include/ode/config.h \ - include/ode/memory.h \ - include/ode/error.h \ - ode/src/array.h -ode/src/error.o: \ - ode/src/error.cpp \ - include/ode/config.h \ - include/ode/error.h -ode/src/memory.o: \ - ode/src/memory.cpp \ - include/ode/config.h \ - include/ode/memory.h \ - include/ode/error.h -ode/src/obstack.o: \ - ode/src/obstack.cpp \ - include/ode/common.h \ - include/ode/config.h \ - include/ode/error.h \ - include/ode/memory.h \ - ode/src/obstack.h \ - ode/src/objects.h \ - include/ode/mass.h \ - ode/src/array.h -ode/src/odemath.o: \ - ode/src/odemath.cpp \ - include/ode/common.h \ - include/ode/config.h \ - include/ode/error.h \ - include/ode/odemath.h -ode/src/matrix.o: \ - ode/src/matrix.cpp \ - include/ode/common.h \ - include/ode/config.h \ - include/ode/error.h \ - include/ode/matrix.h -ode/src/misc.o: \ - ode/src/misc.cpp \ - include/ode/config.h \ - include/ode/misc.h \ - include/ode/common.h \ - include/ode/error.h \ - include/ode/matrix.h -ode/src/rotation.o: \ - ode/src/rotation.cpp \ - include/ode/rotation.h \ - include/ode/common.h \ - include/ode/config.h \ - include/ode/error.h -ode/src/mass.o: \ - ode/src/mass.cpp \ - include/ode/config.h \ - include/ode/mass.h \ - include/ode/common.h \ - include/ode/error.h \ - include/ode/odemath.h \ - include/ode/matrix.h -ode/src/ode.o: \ - ode/src/ode.cpp \ - ode/src/objects.h \ - include/ode/common.h \ - include/ode/config.h \ - include/ode/error.h \ - include/ode/memory.h \ - include/ode/mass.h \ - ode/src/array.h \ - include/ode/ode.h \ - include/ode/contact.h \ - include/ode/odemath.h \ - include/ode/matrix.h \ - include/ode/timer.h \ - include/ode/rotation.h \ - include/ode/space.h \ - include/ode/geom.h \ - include/ode/misc.h \ - include/ode/objects.h \ - include/ode/odecpp.h \ - ode/src/joint.h \ - ode/src/obstack.h \ - ode/src/step.h -ode/src/step.o: \ - ode/src/step.cpp \ - ode/src/objects.h \ - include/ode/common.h \ - include/ode/config.h \ - include/ode/error.h \ - include/ode/memory.h \ - include/ode/mass.h \ - ode/src/array.h \ - ode/src/joint.h \ - include/ode/contact.h \ - ode/src/obstack.h \ - include/ode/odemath.h \ - include/ode/rotation.h \ - include/ode/timer.h \ - include/ode/matrix.h \ - ode/src/lcp.h -ode/src/lcp.o: \ - ode/src/lcp.cpp \ - include/ode/common.h \ - include/ode/config.h \ - include/ode/error.h \ - ode/src/lcp.h \ - include/ode/matrix.h \ - include/ode/misc.h \ - ode/src/mat.h \ - include/ode/timer.h -ode/src/joint.o: \ - ode/src/joint.cpp \ - include/ode/odemath.h \ - include/ode/common.h \ - include/ode/config.h \ - include/ode/error.h \ - include/ode/rotation.h \ - include/ode/matrix.h \ - ode/src/joint.h \ - ode/src/objects.h \ - include/ode/memory.h \ - include/ode/mass.h \ - ode/src/array.h \ - include/ode/contact.h \ - ode/src/obstack.h -ode/src/space.o: \ - ode/src/space.cpp \ - include/ode/common.h \ - include/ode/config.h \ - include/ode/error.h \ - include/ode/space.h \ - include/ode/geom.h \ - include/ode/contact.h \ - include/ode/memory.h \ - ode/src/objects.h \ - include/ode/mass.h \ - ode/src/array.h \ - ode/src/geom_internal.h -ode/src/geom.o: \ - ode/src/geom.cpp \ - include/ode/common.h \ - include/ode/config.h \ - include/ode/error.h \ - include/ode/geom.h \ - include/ode/space.h \ - include/ode/contact.h \ - include/ode/rotation.h \ - include/ode/odemath.h \ - include/ode/memory.h \ - include/ode/misc.h \ - include/ode/objects.h \ - include/ode/mass.h \ - include/ode/matrix.h \ - ode/src/objects.h \ - ode/src/array.h \ - ode/src/geom_internal.h -ode/src/timer.o: \ - ode/src/timer.cpp \ - include/ode/common.h \ - include/ode/config.h \ - include/ode/error.h \ - include/ode/timer.h -ode/src/mat.o: \ - ode/src/mat.cpp \ - include/ode/config.h \ - include/ode/misc.h \ - include/ode/common.h \ - include/ode/error.h \ - include/ode/matrix.h \ - include/ode/memory.h \ - ode/src/mat.h -ode/src/testing.o: \ - ode/src/testing.cpp \ - include/ode/config.h \ - include/ode/misc.h \ - include/ode/common.h \ - include/ode/error.h \ - include/ode/memory.h \ - ode/src/testing.h \ - ode/src/array.h -ode/src/fastldlt.o: \ - ode/src/fastldlt.c \ - include/ode/matrix.h \ - include/ode/common.h \ - include/ode/config.h \ - include/ode/error.h -ode/src/fastlsolve.o: \ - ode/src/fastlsolve.c \ - include/ode/matrix.h \ - include/ode/common.h \ - include/ode/config.h \ - include/ode/error.h -ode/src/fastltsolve.o: \ - ode/src/fastltsolve.c \ - include/ode/matrix.h \ - include/ode/common.h \ - include/ode/config.h \ - include/ode/error.h -ode/src/fastdot.o: \ - ode/src/fastdot.c \ - include/ode/matrix.h \ - include/ode/common.h \ - include/ode/config.h \ - include/ode/error.h -drawstuff/src/drawstuff.o: \ - drawstuff/src/drawstuff.cpp \ - include/ode/config.h \ - include/drawstuff/drawstuff.h \ - include/drawstuff/version.h \ - drawstuff/src/internal.h -drawstuff/src/x11.o: \ - drawstuff/src/x11.cpp \ - include/ode/config.h \ - include/drawstuff/drawstuff.h \ - include/drawstuff/version.h \ - drawstuff/src/internal.h -ode/test/test_ode.o: \ - ode/test/test_ode.cpp \ - include/ode/ode.h \ - include/ode/config.h \ - include/ode/contact.h \ - include/ode/common.h \ - include/ode/error.h \ - include/ode/memory.h \ - include/ode/odemath.h \ - include/ode/matrix.h \ - include/ode/timer.h \ - include/ode/rotation.h \ - include/ode/mass.h \ - include/ode/space.h \ - include/ode/geom.h \ - include/ode/misc.h \ - include/ode/objects.h \ - include/ode/odecpp.h -ode/test/test_chain2.o: \ - ode/test/test_chain2.cpp \ - include/ode/ode.h \ - include/ode/config.h \ - include/ode/contact.h \ - include/ode/common.h \ - include/ode/error.h \ - include/ode/memory.h \ - include/ode/odemath.h \ - include/ode/matrix.h \ - include/ode/timer.h \ - include/ode/rotation.h \ - include/ode/mass.h \ - include/ode/space.h \ - include/ode/geom.h \ - include/ode/misc.h \ - include/ode/objects.h \ - include/ode/odecpp.h \ - include/drawstuff/drawstuff.h \ - include/drawstuff/version.h -ode/test/test_hinge.o: \ - ode/test/test_hinge.cpp \ - include/ode/ode.h \ - include/ode/config.h \ - include/ode/contact.h \ - include/ode/common.h \ - include/ode/error.h \ - include/ode/memory.h \ - include/ode/odemath.h \ - include/ode/matrix.h \ - include/ode/timer.h \ - include/ode/rotation.h \ - include/ode/mass.h \ - include/ode/space.h \ - include/ode/geom.h \ - include/ode/misc.h \ - include/ode/objects.h \ - include/ode/odecpp.h \ - include/drawstuff/drawstuff.h \ - include/drawstuff/version.h -ode/test/test_slider.o: \ - ode/test/test_slider.cpp \ - include/ode/ode.h \ - include/ode/config.h \ - include/ode/contact.h \ - include/ode/common.h \ - include/ode/error.h \ - include/ode/memory.h \ - include/ode/odemath.h \ - include/ode/matrix.h \ - include/ode/timer.h \ - include/ode/rotation.h \ - include/ode/mass.h \ - include/ode/space.h \ - include/ode/geom.h \ - include/ode/misc.h \ - include/ode/objects.h \ - include/ode/odecpp.h \ - include/drawstuff/drawstuff.h \ - include/drawstuff/version.h -ode/test/test_collision.o: \ - ode/test/test_collision.cpp \ - include/ode/ode.h \ - include/ode/config.h \ - include/ode/contact.h \ - include/ode/common.h \ - include/ode/error.h \ - include/ode/memory.h \ - include/ode/odemath.h \ - include/ode/matrix.h \ - include/ode/timer.h \ - include/ode/rotation.h \ - include/ode/mass.h \ - include/ode/space.h \ - include/ode/geom.h \ - include/ode/misc.h \ - include/ode/objects.h \ - include/ode/odecpp.h \ - include/drawstuff/drawstuff.h \ - include/drawstuff/version.h -ode/test/test_boxstack.o: \ - ode/test/test_boxstack.cpp \ - include/ode/ode.h \ - include/ode/config.h \ - include/ode/contact.h \ - include/ode/common.h \ - include/ode/error.h \ - include/ode/memory.h \ - include/ode/odemath.h \ - include/ode/matrix.h \ - include/ode/timer.h \ - include/ode/rotation.h \ - include/ode/mass.h \ - include/ode/space.h \ - include/ode/geom.h \ - include/ode/misc.h \ - include/ode/objects.h \ - include/ode/odecpp.h \ - include/drawstuff/drawstuff.h \ - include/drawstuff/version.h -ode/test/test_buggy.o: \ - ode/test/test_buggy.cpp \ - include/ode/ode.h \ - include/ode/config.h \ - include/ode/contact.h \ - include/ode/common.h \ - include/ode/error.h \ - include/ode/memory.h \ - include/ode/odemath.h \ - include/ode/matrix.h \ - include/ode/timer.h \ - include/ode/rotation.h \ - include/ode/mass.h \ - include/ode/space.h \ - include/ode/geom.h \ - include/ode/misc.h \ - include/ode/objects.h \ - include/ode/odecpp.h \ - include/drawstuff/drawstuff.h \ - include/drawstuff/version.h -ode/test/test_joints.o: \ - ode/test/test_joints.cpp \ - include/ode/ode.h \ - include/ode/config.h \ - include/ode/contact.h \ - include/ode/common.h \ - include/ode/error.h \ - include/ode/memory.h \ - include/ode/odemath.h \ - include/ode/matrix.h \ - include/ode/timer.h \ - include/ode/rotation.h \ - include/ode/mass.h \ - include/ode/space.h \ - include/ode/geom.h \ - include/ode/misc.h \ - include/ode/objects.h \ - include/ode/odecpp.h \ - include/drawstuff/drawstuff.h \ - include/drawstuff/version.h -ode/test/test_space.o: \ - ode/test/test_space.cpp \ - include/ode/ode.h \ - include/ode/config.h \ - include/ode/contact.h \ - include/ode/common.h \ - include/ode/error.h \ - include/ode/memory.h \ - include/ode/odemath.h \ - include/ode/matrix.h \ - include/ode/timer.h \ - include/ode/rotation.h \ - include/ode/mass.h \ - include/ode/space.h \ - include/ode/geom.h \ - include/ode/misc.h \ - include/ode/objects.h \ - include/ode/odecpp.h \ - include/drawstuff/drawstuff.h \ - include/drawstuff/version.h -ode/test/test_I.o: \ - ode/test/test_I.cpp \ - include/ode/ode.h \ - include/ode/config.h \ - include/ode/contact.h \ - include/ode/common.h \ - include/ode/error.h \ - include/ode/memory.h \ - include/ode/odemath.h \ - include/ode/matrix.h \ - include/ode/timer.h \ - include/ode/rotation.h \ - include/ode/mass.h \ - include/ode/space.h \ - include/ode/geom.h \ - include/ode/misc.h \ - include/ode/objects.h \ - include/ode/odecpp.h \ - include/drawstuff/drawstuff.h \ - include/drawstuff/version.h -ode/test/test_step.o: \ - ode/test/test_step.cpp \ - include/ode/ode.h \ - include/ode/config.h \ - include/ode/contact.h \ - include/ode/common.h \ - include/ode/error.h \ - include/ode/memory.h \ - include/ode/odemath.h \ - include/ode/matrix.h \ - include/ode/timer.h \ - include/ode/rotation.h \ - include/ode/mass.h \ - include/ode/space.h \ - include/ode/geom.h \ - include/ode/misc.h \ - include/ode/objects.h \ - include/ode/odecpp.h \ - include/drawstuff/drawstuff.h \ - include/drawstuff/version.h -ode/test/test_friction.o: \ - ode/test/test_friction.cpp \ - include/ode/ode.h \ - include/ode/config.h \ - include/ode/contact.h \ - include/ode/common.h \ - include/ode/error.h \ - include/ode/memory.h \ - include/ode/odemath.h \ - include/ode/matrix.h \ - include/ode/timer.h \ - include/ode/rotation.h \ - include/ode/mass.h \ - include/ode/space.h \ - include/ode/geom.h \ - include/ode/misc.h \ - include/ode/objects.h \ - include/ode/odecpp.h \ - include/drawstuff/drawstuff.h \ - include/drawstuff/version.h -drawstuff/dstest/dstest.o: \ - drawstuff/dstest/dstest.cpp \ - include/drawstuff/drawstuff.h \ - include/drawstuff/version.h diff --git a/extern/ode/dist/README b/extern/ode/dist/README deleted file mode 100644 index 6b151d1a27b..00000000000 --- a/extern/ode/dist/README +++ /dev/null @@ -1,30 +0,0 @@ -The Open Dynamics Engine (ODE), Copyright (C) 2001,2002 Russell L. Smith. -------------------------------------------------------------------------- - -ODE is a free, industrial quality library for simulating articulated -rigid body dynamics - for example ground vehicles, legged creatures, -and moving objects in VR environments. It is fast, flexible, robust -and platform independent, with advanced joints, contact with friction, -and built-in collision detection. - -This library is free software; you can redistribute it and/or -modify it under the terms of EITHER: - (1) The GNU Lesser General Public License as published by the Free - Software Foundation; either version 2.1 of the License, or (at - your option) any later version. The text of the GNU Lesser - General Public License is included with this library in the - file LICENSE.TXT. - (2) The BSD-style license that is included with this library in - the file LICENSE-BSD.TXT. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files -LICENSE.TXT and LICENSE-BSD.TXT for more details. - - * Installation instructions are in the INSTALL file - - * The ODE web pages are at http://q12.org/ode/ - - * The ODE documentation is in the file ode/doc/ode.html, or you - can view it on the web at http://q12.org/ode/ode-docs.html diff --git a/extern/ode/dist/README_BLENDER b/extern/ode/dist/README_BLENDER deleted file mode 100644 index 64d97624a55..00000000000 --- a/extern/ode/dist/README_BLENDER +++ /dev/null @@ -1,18 +0,0 @@ -Checked out from ODE cvs (http://q12.org) on or around -Fri Oct 18 14:24:11 GMT 2002 - -Note that ODE has its own build system. The source/ode/ -directory is traversed by Blender's source/Makefile. However -source/ode/config/user-settings has to be set correctly -depending on the target platform. This needs to be done in the -source/Makefile which determines the proper platform, then copies -the appropriate platform-specific user-settings.platform file to -source/ode/config/user-settings. Currently source/ode/user-settings is -for Linux. - -Don't change the source in this directory. This source code is -maintained on the CVS server at http://q12.org and is provided here -so that Blender developers can compile ODE without having to separately -download and install it. This source code can and should periodically -be synchronized (manually) with the source code at http://q12.org. - diff --git a/extern/ode/dist/config/README b/extern/ode/dist/config/README deleted file mode 100644 index 0fa18062a4e..00000000000 --- a/extern/ode/dist/config/README +++ /dev/null @@ -1,41 +0,0 @@ - -variable names used in the per-platform makefile configuration files: - -platform stuff --------------- - -WINDOWS set to 1 if this is a microsoft windows based platform - -filesystem stuff and commands ------------------------------ - -THIS_DIR prefix to run a command from the current directory -DEL_CMD the name of the delete command - -compiler stuff --------------- - -CC the C/C++ compiler to use -OBJ the object file extension -C_FLAGS the standard set of compiler flags -C_INC flag to add an include path -C_OUT flag to specify the object file output -C_EXEOUT flag to specify the executable file output -C_DEF flag to add a define -C_OPT flag to set the optimization level -OPT the optimization level to use - -library archiver ----------------- - -AR library archiver command -RANLIB ranlib command, if necessary -LIB_PREFIX library file prefix -LIB_SUFFIX library file suffix -LINK_OPENGL link flags to link in windowing stuff and opengl -LINK_MATH link flags to link in the system math library - -windows specific stuff ----------------------- - -RC_RULE makefile rule to use for the resource compiler diff --git a/extern/ode/dist/config/makefile.cygwin b/extern/ode/dist/config/makefile.cygwin deleted file mode 100644 index de23b71a29f..00000000000 --- a/extern/ode/dist/config/makefile.cygwin +++ /dev/null @@ -1,28 +0,0 @@ -WINDOWS=1 -THIS_DIR=./ -DEL_CMD=rm -f -CC=gcc -OBJ=.o -C_FLAGS=-c -Wall -fno-exceptions -fno-rtti -DWIN32 -DCYGWIN -C_INC=-I -C_OUT=-o -C_EXEOUT=-o -C_DEF=-D -C_OPT=-O -AR=ar rc -RANLIB= -LIB_PREFIX=lib -LIB_SUFFIX=.a -LINK_OPENGL=-lComctl32 -lkernel32 -luser32 -lgdi32 -lOpenGL32 -lGlu32 -LINK_MATH=-lm -RC_RULE=windres -I rc -O coff $< $@ - -ifeq ($(BUILD),release) -OPT=2 -C_FLAGS+=-fomit-frame-pointer -ffast-math -endif - -ifeq ($(BUILD),debug) -OPT=0 -C_FLAGS+=-g -endif diff --git a/extern/ode/dist/config/makefile.mingw b/extern/ode/dist/config/makefile.mingw deleted file mode 100644 index 4b18fb6bcdc..00000000000 --- a/extern/ode/dist/config/makefile.mingw +++ /dev/null @@ -1,28 +0,0 @@ -WINDOWS=1 -THIS_DIR= -DEL_CMD=tools\rm -CC=gcc -OBJ=.o -C_FLAGS=-c -Wall -fno-exceptions -fno-rtti -DWIN32 -C_INC=-I -C_OUT=-o -C_EXEOUT=-o -C_DEF=-D -C_OPT=-O -AR=ar rc -RANLIB= -LIB_PREFIX=lib -LIB_SUFFIX=.a -LINK_OPENGL=-lComctl32 -lkernel32 -luser32 -lgdi32 -lOpenGL32 -lGlu32 -LINK_MATH=-lm -RC_RULE=windres -I rc -O coff $< $@ - -ifeq ($(BUILD),release) -OPT=2 -C_FLAGS+=-fomit-frame-pointer -ffast-math -endif - -ifeq ($(BUILD),debug) -OPT=0 -C_FLAGS+=-g -endif diff --git a/extern/ode/dist/config/makefile.msvc b/extern/ode/dist/config/makefile.msvc deleted file mode 100644 index 9d4da0bf912..00000000000 --- a/extern/ode/dist/config/makefile.msvc +++ /dev/null @@ -1,27 +0,0 @@ -WINDOWS=1 -THIS_DIR= -DEL_CMD=tools\rm -CC=cl /nologo /DWIN32 /DMSVC /DSHAREDLIBEXPORT= /DSHAREDLIBIMPORT= -OBJ=.obj -C_FLAGS=/c /GR- /GX- /W3 /GF -C_INC=/I -C_OUT=/Fo -C_EXEOUT=/Fe -C_DEF=/D -C_OPT=/O -AR=lib /nologo /OUT: -RANLIB= -LIB_PREFIX= -LIB_SUFFIX=.lib -LINK_OPENGL=Comctl32.lib kernel32.lib user32.lib gdi32.lib OpenGL32.lib Glu32.lib -LINK_MATH= -RC_RULE=rc /r /fo$@ $< - -ifeq ($(BUILD),release) -OPT=2 -C_FLAGS+=/Oy -endif - -ifeq ($(BUILD),debug) -OPT=d -endif diff --git a/extern/ode/dist/config/makefile.msvc-dll b/extern/ode/dist/config/makefile.msvc-dll deleted file mode 100644 index fe495893616..00000000000 --- a/extern/ode/dist/config/makefile.msvc-dll +++ /dev/null @@ -1,29 +0,0 @@ -WINDOWS=1 -THIS_DIR= -DEL_CMD=tools\rm -CC=cl /nologo /DWIN32 /DMSVC /DSHAREDLIBIMPORT=__declspec(dllimport) /DSHAREDLIBEXPORT=__declspec(dllexport) -OBJ=.obj -C_FLAGS=/c /GR- /GX- /W3 /GF -C_INC=/I -C_OUT=/Fo -C_EXEOUT=/Fe -C_DEF=/D -C_OPT=/O -AR=lib /nologo /OUT: -RANLIB= -LIB_PREFIX= -LIB_SUFFIX=.lib -LINK_OPENGL=Comctl32.lib kernel32.lib user32.lib gdi32.lib OpenGL32.lib Glu32.lib -LINK_MATH= -RC_RULE=rc /r /fo$@ $< - -ifeq ($(BUILD),release) -OPT=2 -C_FLAGS+=/Oy -endif - -ifeq ($(BUILD),debug) -OPT=d -endif - -ODE_LIB_AR_RULE=link /dll /nologo /SUBSYSTEM:WINDOWS /LIBPATH:"C:\Programme\Micros~2\VC98\Lib" /def:config/msvcdefs.def $(LINK_OPENGL) /OUT:$(patsubst %.lib,%.dll,$@) diff --git a/extern/ode/dist/config/makefile.osx b/extern/ode/dist/config/makefile.osx deleted file mode 100644 index 1d5e0f42252..00000000000 --- a/extern/ode/dist/config/makefile.osx +++ /dev/null @@ -1,26 +0,0 @@ -THIS_DIR=./ -DEL_CMD=rm -f -CC=cc -OBJ=.o -C_FLAGS=-c -Wall -fno-rtti -fno-exceptions -Wall -I/usr/X11R6/include -C_INC=-I -C_OUT=-o -C_EXEOUT=-o -C_DEF=-D -C_OPT=-O -AR=ar rc -RANLIB=ranlib -LIB_PREFIX=lib -LIB_SUFFIX=.a -LINK_OPENGL=-L/usr/X11R6/lib -L/usr/X11/lib -L/usr/lib/X11R6 -L/usr/lib/X11 -lX11 -lGL -lGLU -lstdc++ -LINK_MATH=-lm - -ifeq ($(BUILD),release) -OPT=2 -C_FLAGS+=-fomit-frame-pointer -ffast-math -endif - -ifeq ($(BUILD),debug) -OPT=0 -C_FLAGS+=-g -endif diff --git a/extern/ode/dist/config/makefile.unix-gcc b/extern/ode/dist/config/makefile.unix-gcc deleted file mode 100644 index b9d07632353..00000000000 --- a/extern/ode/dist/config/makefile.unix-gcc +++ /dev/null @@ -1,29 +0,0 @@ -THIS_DIR=./ -DEL_CMD=rm -f -CC=gcc -OBJ=.o -C_FLAGS=-c -Wall -fno-rtti -fno-exceptions -Wall -C_INC=-I -C_OUT=-o -C_EXEOUT=-o -C_DEF=-D -C_OPT=-O -AR=ar rc -RANLIB= -LIB_PREFIX=lib -LIB_SUFFIX=.a -LINK_OPENGL=-L/usr/X11R6/lib -L/usr/X11/lib -L/usr/lib/X11R6 -L/usr/lib/X11 -lX11 -lGL -lGLU -LINK_MATH=-lm - -ifeq ($(BUILD),release) -OPT=2 -C_FLAGS+=-fomit-frame-pointer -ffast-math -endif - -ifeq ($(BUILD),debug) -OPT=0 -C_FLAGS+=-g -endif - -# some other possible flags: -# -malign-double -mpentiumpro -march=pentiumpro diff --git a/extern/ode/dist/config/makefile.unix-generic b/extern/ode/dist/config/makefile.unix-generic deleted file mode 100644 index f435e1a0db8..00000000000 --- a/extern/ode/dist/config/makefile.unix-generic +++ /dev/null @@ -1,24 +0,0 @@ -THIS_DIR=./ -DEL_CMD=rm -f -CC=CC -OBJ=.o -C_FLAGS=-c -C_INC=-I -C_OUT=-o -C_EXEOUT=-o -C_DEF=-D -C_OPT=-O -AR=ar rc -RANLIB= -LIB_PREFIX=lib -LIB_SUFFIX=.a -LINK_OPENGL=-L/usr/X11R6/lib -L/usr/X11/lib -L/usr/lib/X11R6 -L/usr/lib/X11 -lX11 -lGL -lGLU -LINK_MATH=-lm - -ifeq ($(BUILD),release) -OPT=2 -endif - -ifeq ($(BUILD),debug) -OPT=0 -endif diff --git a/extern/ode/dist/config/msvcdefs.def b/extern/ode/dist/config/msvcdefs.def deleted file mode 100644 index 11258ab9aa8..00000000000 --- a/extern/ode/dist/config/msvcdefs.def +++ /dev/null @@ -1,228 +0,0 @@ -LIBRARY ODE -EXPORTS -dAreConnected -dBodyAddForce -dBodyAddForceAtPos -dBodyAddForceAtRelPos -dBodyAddRelForce -dBodyAddRelForceAtPos -dBodyAddRelForceAtRelPos -dBodyAddRelTorque -dBodyAddTorque -dBodyCreate -dBodyDestroy -dBodyDisable -dBodyEnable -dBodyGetAngularVel -dBodyGetData -dBodyGetFiniteRotationAxis -dBodyGetFiniteRotationMode -dBodyGetForce -dBodyGetGravityMode -dBodyGetJoint -dBodyGetLinearVel -dBodyGetMass -dBodyGetNumJoints -dBodyGetPointVel -dBodyGetPosRelPoint -dBodyGetPosition -dBodyGetQuaternion -dBodyGetRelPointPos -dBodyGetRelPointVel -dBodyGetRotation -dBodyGetTorque -dBodyIsEnabled -dBodySetAngularVel -dBodySetData -dBodySetFiniteRotationAxis -dBodySetFiniteRotationMode -dBodySetForce -dBodySetGravityMode -dBodySetLinearVel -dBodySetMass -dBodySetPosition -dBodySetQuaternion -dBodySetRotation -dBodySetTorque -dBodyVectorFromWorld -dBodyVectorToWorld -dBoxBox -dBoxClass -dBoxTouchesBox -dCCylinderClass -dClearUpperTriangle -dCloseODE -dClosestLineSegmentPoints -dCollide -dCreateBox -dCreateCCylinder -dCreateGeom -dCreateGeomClass -dCreateGeomGroup -dCreateGeomTransform -dCreatePlane -dCreateSphere -dError -dFactorCholesky -dFactorLDLT -dGeomBoxGetLengths -dGeomBoxSetLengths -dGeomCCylinderGetParams -dGeomCCylinderSetParams -dGeomDestroy -dGeomGetAABB -dGeomGetBody -dGeomGetClass -dGeomGetClassData -dGeomGetData -dGeomGetPosition -dGeomGetRotation -dGeomGetSpaceAABB -dGeomGroupAdd -dGeomGroupGetGeom -dGeomGroupGetNumGeoms -dGeomGroupRemove -dGeomPlaneGetParams -dGeomPlaneSetParams -dGeomSetBody -dGeomSetData -dGeomSetPosition -dGeomSetRotation -dGeomSphereGetRadius -dGeomSphereSetRadius -dGeomTransformClass -dGeomTransformGetCleanup -dGeomTransformGetGeom -dGeomTransformSetCleanup -dGeomTransformSetGeom -dHashSpaceCreate -dHashSpaceSetLevels -dInfiniteAABB -dInfinityValue -dInvertPDMatrix -dIsPositiveDefinite -dJointAttach -dJointCreateAMotor -dJointCreateBall -dJointCreateContact -dJointCreateFixed -dJointCreateHinge -dJointCreateHinge2 -dJointCreateSlider -dJointCreateUniversal -dJointDestroy -dJointGetAMotorAngle -dJointGetAMotorAngleRate -dJointGetAMotorAxis -dJointGetAMotorAxisRel -dJointGetAMotorMode -dJointGetAMotorNumAxes -dJointGetAMotorParam -dJointGetBallAnchor -dJointGetBody -dJointGetData -dJointGetHinge2Anchor -dJointGetHinge2Angle1 -dJointGetHinge2Angle1Rate -dJointGetHinge2Angle2Rate -dJointGetHinge2Axis1 -dJointGetHinge2Axis2 -dJointGetHinge2Param -dJointGetHingeAnchor -dJointGetHingeAngle -dJointGetHingeAngleRate -dJointGetHingeAxis -dJointGetHingeParam -dJointGetSliderAxis -dJointGetSliderParam -dJointGetSliderPosition -dJointGetSliderPositionRate -dJointGetType -dJointGetUniversalAnchor -dJointGetUniversalAxis1 -dJointGetUniversalAxis2 -dJointGroupCreate -dJointGroupDestroy -dJointGroupEmpty -dJointSetAMotorAngle -dJointSetAMotorAxis -dJointSetAMotorMode -dJointSetAMotorNumAxes -dJointSetAMotorParam -dJointSetBallAnchor -dJointSetData -dJointSetFixed -dJointSetHinge2Anchor -dJointSetHinge2Axis1 -dJointSetHinge2Axis2 -dJointSetHinge2Param -dJointSetHingeAnchor -dJointSetHingeAxis -dJointSetHingeParam -dJointSetSliderAxis -dJointSetSliderParam -dJointSetUniversalAnchor -dJointSetUniversalAxis1 -dJointSetUniversalAxis2 -dLDLTAddTL -dLDLTRemove -dMakeRandomMatrix -dMakeRandomVector -dMassAdd -dMassAdjust -dMassRotate -dMassSetBox -dMassSetCappedCylinder -dMassSetParameters -dMassSetSphere -dMassSetZero -dMassTranslate -dMaxDifference -dMultiply0 -dMultiply1 -dMultiply2 -dNormalize3 -dNormalize4 -dPlaneSpace -dQFromAxisAndAngle -dQMultiply0 -dQMultiply1 -dQMultiply2 -dQMultiply3 -dQSetIdentity -dQtoR -dRFrom2Axes -dRFromAxisAndAngle -dRFromEulerAngles -dRSetIdentity -dRandInt -dRandReal -dRandSetSeed -dRemoveRowCol -dRtoQ -dSetMessageHandler -dSetZero -dSimpleSpaceCreate -dSolveCholesky -dSolveLDLT -dSpaceAdd -dSpaceCollide -dSpaceDestroy -dSpaceQuery -dSpaceRemove -dSphereClass -dTestMatrixComparison -dTestRand -dTestSolveLCP -dWorldCreate -dWorldDestroy -dWorldGetCFM -dWorldGetERP -dWorldGetGravity -dWorldImpulseToForce -dWorldSetCFM -dWorldSetERP -dWorldSetGravity -dWorldStep -dWorldStep -dWtoDQ diff --git a/extern/ode/dist/config/user-settings b/extern/ode/dist/config/user-settings deleted file mode 100644 index d632eba9678..00000000000 --- a/extern/ode/dist/config/user-settings +++ /dev/null @@ -1,31 +0,0 @@ -# ODE user settings: the following variables must be set by the user - -# (1) the platform to use. this name should have a corresponding -# makefile.PLATFORM file. currently supported platforms are: -# msvc microsoft visual C/C++ -# msvc-dll microsoft visual C/C++, create a DLL -# mingw minimalist GNU for windows -# cygwin cygnus GNU for windows -# unix-gcc GNU gcc on unix -# unix-generic generic unix compiler. you may need to edit the CC -# variable in makefile.unix-generic -# osx Mac OS-X, with the gnu compiler. - -PLATFORM=unix-gcc - -# (2) the floating point precision to use (either "SINGLE" or "DOUBLE") - -PRECISION=SINGLE -#PRECISION=DOUBLE - -# (3) the library type to build (either "debug" if you are doing development, -# or "release" for the optimized library) - -#BUILD=debug -BUILD=release - -# (4) if you are using an old version of MS-Windows that has command line -# length limitations then you will need to set this to "1". otherwise, -# leave it at "0". - -WINDOWS16=0 diff --git a/extern/ode/dist/config/user-settings.example b/extern/ode/dist/config/user-settings.example deleted file mode 100644 index 0b0d480a25a..00000000000 --- a/extern/ode/dist/config/user-settings.example +++ /dev/null @@ -1,31 +0,0 @@ -# ODE user settings: the following variables must be set by the user - -# (1) the platform to use. this name should have a corresponding -# makefile.PLATFORM file. currently supported platforms are: -# msvc microsoft visual C/C++ -# msvc-dll microsoft visual C/C++, create a DLL -# mingw minimalist GNU for windows -# cygwin cygnus GNU for windows -# unix-gcc GNU gcc on unix -# unix-generic generic unix compiler. you may need to edit the CC -# variable in makefile.unix-generic -# osx Mac OS-X, with the gnu compiler. - -PLATFORM=unix-gcc - -# (2) the floating point precision to use (either "SINGLE" or "DOUBLE") - -#PRECISION=SINGLE -PRECISION=DOUBLE - -# (3) the library type to build (either "debug" if you are doing development, -# or "release" for the optimized library) - -#BUILD=debug -BUILD=release - -# (4) if you are using an old version of MS-Windows that has command line -# length limitations then you will need to set this to "1". otherwise, -# leave it at "0". - -WINDOWS16=0 diff --git a/extern/ode/dist/configurator.c b/extern/ode/dist/configurator.c deleted file mode 100644 index 8129716881d..00000000000 --- a/extern/ode/dist/configurator.c +++ /dev/null @@ -1,437 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -/* - -this program discovers some system configuration stuff, prior to compiling -ODE. the usage is: - - configurator <config.h-file-to-generate> <compiler-command-line> - <delete-command-line> <THIS_DIR-variable> - -this program looks long, but it really has an extremely simple structure and -should be very easy for anyone to modify. it should be very portable as it -is written in straight ANSI C and only uses the following library functions: - * printf - * fopen (assumes 0 returned on failure) - * fclose - * fprintf - * system - * exit -except where stated, we do not assume anything about the return codes from -these functions. - -why didn't i just use GNU autoconf? : - * autoconf needs a bourne shell and a bunch of other tools that windows - users may not have. - * i like reinventing the wheel. - -*/ - -#include <stdio.h> - -/****************************************************************************/ -/* project constants */ - -#define SETUP_SHLIB_DEFS \ - "#ifndef SHAREDLIBIMPORT\n" \ - "#define SHAREDLIBIMPORT\n" \ - "#endif\n" \ - "#ifndef SHAREDLIBEXPORT\n" \ - "#define SHAREDLIBEXPORT\n" \ - "#endif\n" - -/* the config.h header */ -char *config_h_part1 = -"/* per-machine configuration. this file is automatically generated. */\n" -"\n" -"#ifndef _ODE_CONFIG_H_\n" -"#define _ODE_CONFIG_H_\n" -"\n" -"/* shared lib definitions */\n" -SETUP_SHLIB_DEFS -"\n" -"/* standard system headers */\n"; - - -char *config_h_part2 = -"\n" -"#ifdef __cplusplus\n" -"extern \"C\" {\n" -"#endif\n" -"\n"; - -/* the config.h footer */ -char *config_h_footer = -"#ifdef __cplusplus\n" -"}\n" -"#endif\n" -"#endif\n"; - -/****************************************************************************/ -/* implementations of some string functions. these are prefixed with 'x' - * to prevent any conflicts with built-in functions. - */ - -#define strcpy xstrcpy -void xstrcpy (char *dest, char *src) -{ - while (*src) *dest++ = *src++; - *dest = 0; -} - - -#define strcat xstrcat -void xstrcat (char *dest, char *src) -{ - while (*dest) dest++; - while (*src) *dest++ = *src++; - *dest = 0; -} - -/****************************************************************************/ -/* utility functions */ - -/* print an error message and exit */ - -void fatal_error (char *message) -{ - printf ("\n*** configurator failed: %s.\n\n" - "please fix your configuration and try again.\n" - "if you have to fix the configurator program or the makefiles, " - "please email\n" - "your changes to the ODE mailing list (ode@q12.org).\n\n", message); - exit (0); -} - - -/* open a file, generate an error if it can't be done */ - -FILE * xfopen (char *filename, char *mode) -{ - FILE *f; - f = fopen (filename,mode); - if (!f) fatal_error ("can not open a file"); - return f; -} - - -/* return 1 if the file exists or 0 if not */ - -int file_exists (char *filename) -{ - FILE *f; - f = fopen (filename,"rb"); - if (f) fclose (f); - return (f != 0); -} - - -/* write a string to a new file */ - -void write_to_file (char *filename, char *s) -{ - FILE *f = xfopen (filename,"wt"); - fprintf (f,"%s",s); - fclose (f); -} - - -/* write a comment to a header file */ - -void write_header_comment (FILE *file, char *description) -{ - fprintf (file,"/* %s */\n",description); - printf ("%s ...\n",description); -} - - -/* delete a file */ - -char *delete_cmd_line = 0; -void delete_file (char *filename) -{ - char cmd[1000]; - strcpy (cmd,delete_cmd_line); - strcat (cmd," "); - strcat (cmd,filename); - printf ("%s\n",cmd); - system (cmd); -} - - -/* run a compile command */ - -char *compile_cmd_line = 0; -void compile (char *output, char *input) -{ - char cmd[1000]; - strcpy (cmd,compile_cmd_line); - strcat (cmd,output); - strcat (cmd," "); - strcat (cmd,input); - printf ("%s\n",cmd); - system (cmd); -} - - -/* run a program we've just compiled */ - -char *run_prefix = ""; -void run (char *filename) -{ - char cmd[1000]; - strcpy (cmd,run_prefix); - strcat (cmd,filename); - printf ("%s\n",cmd); - system (cmd); -} - -/****************************************************************************/ -/* system tests */ - -void check_if_this_is_a_pentium (FILE *file) -{ - write_header_comment (file,"is this a pentium on a gcc-based platform?"); - write_to_file ("ctest.c", - "int main() {\n" - " asm (\"mov $0,%%eax\\n cpuid\\n\" : : : \"%eax\");\n" - " return 0;\n" - "}\n"); - delete_file ("ctest.exe"); - compile ("ctest.exe","ctest.c"); - if (file_exists ("ctest.exe")) { - fprintf (file,"#define PENTIUM 1\n\n"); - } - else { - fprintf (file,"/* #define PENTIUM 1 -- not a pentium */\n\n"); - } - - delete_file ("ctest.c"); - delete_file ("ctest.exe"); -} - -/****************************************************************************/ -/* tests: standard headers */ - -void get_all_standard_headers (FILE *file) -{ - int i; - FILE *f; - char *header[7] = {"stdio.h", "stdlib.h", "math.h", "string.h", - "stdarg.h", "malloc.h", "alloca.h"}; - - for (i=0; i < sizeof(header)/sizeof(char*); i++) { - FILE *f = xfopen ("ctest.c","wt"); - fprintf (f,"#include <%s>\nint main() { return 0; }\n",header[i]); - fclose (f); - delete_file ("ctest.exe"); - compile ("ctest.exe","ctest.c"); - if (file_exists ("ctest.exe")) { - fprintf (file,"#include <%s>\n",header[i]); - } - } - - delete_file ("ctest.c"); - delete_file ("ctest.exe"); -} - -/****************************************************************************/ -/* tests: typedefs and constants for ODE */ - -void get_ODE_integer_typedefs (FILE *file) -{ - write_header_comment (file,"integer types (we assume int >= 32 bits)"); - if (sizeof(char) != 1) fatal_error ("expecting sizeof(char) == 1"); - if (sizeof(int) < 4) fatal_error ("expecting sizeof(int) >= 4"); - fprintf (file,"typedef char int8;\ntypedef unsigned char uint8;\n"); - if (sizeof(short) == 4) { - fprintf (file,"typedef short int32;\ntypedef unsigned short uint32;\n"); - } - else if (sizeof(int) == 4) { - fprintf (file,"typedef int int32;\ntypedef unsigned int uint32;\n"); - } - else { - fatal_error ("can not find 4 byte integer type"); - } - fprintf (file,"\n" - "/* an integer type that we can safely cast a pointer to and\n" - " * from without loss of bits.\n" - " */\n"); - if (sizeof(short) == sizeof(void*)) { - fprintf (file,"typedef unsigned short intP;\n"); - } - else if (sizeof(int) == sizeof(void*)) { - fprintf (file,"typedef unsigned int intP;\n"); - } - else if (sizeof(long int) == sizeof(void*)) { - fprintf (file,"typedef unsigned long int intP;\n"); - } - fprintf (file,"\n"); -} - - -void get_ODE_float_stuff (FILE *file) -{ - char *suffix,*type; - int i; - FILE *f; - -#define SHARED_LIB_SPEC_DECISION \ - "#if defined SHARED_CONFIG_H_INCLUDED_FROM_DEFINING_FILE\n" \ - " #define GLOBAL_SHAREDLIB_SPEC SHAREDLIBEXPORT\n" \ - "#else \n" \ - " #define GLOBAL_SHAREDLIB_SPEC SHAREDLIBIMPORT\n" \ - "#endif\n" - -#define UNDEF_SHAREDLIB_SPEC "\n#undef GLOBAL_SHAREDLIB_SPEC\n" - -#ifdef dSINGLE - -#define INFBYTES SHARED_LIB_SPEC_DECISION "union dInfBytes { unsigned char c[4]; float f; };\nextern GLOBAL_SHAREDLIB_SPEC union dInfBytes dInfinityValue;\n#define dInfinity (dInfinityValue.f)" - - char *inc[6] = {"#include <math.h>", - "#include <math.h>", - "", - "", - "", - ""}; - char *decl[6] = { - "SHAREDLIBEXPORT double dInfinityValue = HUGE_VALF;", - "SHAREDLIBEXPORT double dInfinityValue = HUGE_VAL;", - "SHAREDLIBEXPORT union dInfBytes dInfinityValue = {{0x7f,0x80,0,0}};", - "SHAREDLIBEXPORT union dInfBytes dInfinityValue = {{0,0,0x80,0x7f}};", - "SHAREDLIBEXPORT double dInfinityValue = 1.0f/0.0f;", - "SHAREDLIBEXPORT double dInfinityValue = 1e20f;"}; - char *inf[6] = { - SHARED_LIB_SPEC_DECISION "extern GLOBAL_SHAREDLIB_SPEC double dInfinityValue;\n#define dInfinity dInfinityValue" UNDEF_SHAREDLIB_SPEC, - SHARED_LIB_SPEC_DECISION "extern GLOBAL_SHAREDLIB_SPEC double dInfinityValue;\n#define dInfinity dInfinityValue" UNDEF_SHAREDLIB_SPEC, - INFBYTES UNDEF_SHAREDLIB_SPEC, - INFBYTES UNDEF_SHAREDLIB_SPEC, - SHARED_LIB_SPEC_DECISION "extern GLOBAL_SHAREDLIB_SPEC double dInfinityValue;\n#define dInfinity dInfinityValue" UNDEF_SHAREDLIB_SPEC, - SHARED_LIB_SPEC_DECISION "extern GLOBAL_SHAREDLIB_SPEC double dInfinityValue;\n#define dInfinity dInfinityValue" UNDEF_SHAREDLIB_SPEC}; - -#else /* not dSINGLE, must be dDOUBLE */ - -#define INFBYTES SHARED_LIB_SPEC_DECISION "union dInfBytes { unsigned char c[8]; double d; };\nextern GLOBAL_SHAREDLIB_SPEC union dInfBytes dInfinityValue;\n#define dInfinity (dInfinityValue.d)" - - char *inc[5] = { - "#include <math.h>", - "", - "", - "", - ""}; - char *decl[5] = { - "SHAREDLIBEXPORT double dInfinityValue = HUGE_VAL;", - "SHAREDLIBEXPORT union dInfBytes dInfinityValue = {{0x7f,0xf0,0,0,0,0,0,0}};", - "SHAREDLIBEXPORT union dInfBytes dInfinityValue = {{0,0,0,0,0,0,0xf0,0x7f}};", - "SHAREDLIBEXPORT double dInfinityValue = 1.0/0.0;", - "SHAREDLIBEXPORT double dInfinityValue = 1e20;"}; - char *inf[5] = { - SHARED_LIB_SPEC_DECISION "extern GLOBAL_SHAREDLIB_SPEC double dInfinityValue;\n#define dInfinity dInfinityValue" UNDEF_SHAREDLIB_SPEC, - INFBYTES UNDEF_SHAREDLIB_SPEC, - INFBYTES UNDEF_SHAREDLIB_SPEC, - SHARED_LIB_SPEC_DECISION "extern GLOBAL_SHAREDLIB_SPEC double dInfinityValue;\n#define dInfinity dInfinityValue" UNDEF_SHAREDLIB_SPEC, - SHARED_LIB_SPEC_DECISION "extern GLOBAL_SHAREDLIB_SPEC double dInfinityValue;\n#define dInfinity dInfinityValue" UNDEF_SHAREDLIB_SPEC}; -#endif - - write_header_comment (file,"select the base floating point type"); -#ifdef dSINGLE - fprintf (file,"#define dSINGLE 1\n\n"); - type = "float"; - suffix = "f"; -#else - fprintf (file,"#define dDOUBLE 1\n\n"); - type = "double"; - suffix = ""; -#endif - - /* infinity */ - write_header_comment (file,"the floating point infinity"); - - /* try the different infinity constants until one works */ - for (i=0; i < sizeof(inf)/sizeof(char*); i++) { - f = xfopen ("ctest.c","wt"); - fprintf (f, - "#include <stdio.h>\n" - "#define SHARED_CONFIG_H_INCLUDED_FROM_DEFINING_FILE 1\n" - SETUP_SHLIB_DEFS - "%s\n" - "%s\n" - "%s\n" - "int main() {\n" - " if (dInfinity > 1e10%s && -dInfinity < -1e10%s &&\n" - " -dInfinity < dInfinity) {\n" - " FILE *f = fopen (\"data\",\"wt\");\n" - " fprintf (f,\"foo\\n\");\n" - " fclose (f);\n" - " }\n" - " return 0;\n" - "}\n" - ,inc[i],inf[i],decl[i],suffix,suffix); - fclose (f); - delete_file ("data"); - compile ("ctest.exe","ctest.c"); - run ("ctest.exe"); - if (file_exists ("data")) { - fprintf (file,"#define DINFINITY_DECL %s\n",decl[i]); - fprintf (file,"%s\n\n",inf[i]); - delete_file ("ctest.c"); - delete_file ("ctest.exe"); - delete_file ("data"); - return; - } - } - - fatal_error ("can't determine dInfinity constant"); -} - -/****************************************************************************/ - -int main (int argc, char **argv) -{ - FILE *file; - - if (argc < 4 || argc > 5) - fatal_error ("configurator expects 3 or 4 arguments"); - compile_cmd_line = argv[2]; - delete_cmd_line = argv[3]; - if (argc >= 5) run_prefix = argv[4]; - - /* check some defines we should have been compiled with */ -#if !defined(dSINGLE) && !defined(dDOUBLE) - fatal_error ("you must set PRECISION to either SINGLE or DOUBLE"); -#endif - - file = xfopen (argv[1],"wt"); - fprintf (file,config_h_part1); - get_all_standard_headers (file); - fprintf (file,config_h_part2); - check_if_this_is_a_pentium (file); - get_ODE_integer_typedefs (file); - get_ODE_float_stuff (file); - fprintf (file,config_h_footer); - fclose (file); - - printf ("\n*** configurator succeeded ***\n\n"); - return 0; -} diff --git a/extern/ode/dist/include/ode/README b/extern/ode/dist/include/ode/README deleted file mode 100644 index aaedfcc38fe..00000000000 --- a/extern/ode/dist/include/ode/README +++ /dev/null @@ -1,18 +0,0 @@ - -this is the public C interface to the ODE library. - -all these files should be includable from C, i.e. they should not use any -C++ features. everything should be protected with - - #ifdef __cplusplus - extern "C" { - #endif - - ... - - #ifdef __cplusplus - } - #endif - -the only exception is the odecpp.h file, which defines a C++ wrapper for -the C interface. remember to keep this in sync! diff --git a/extern/ode/dist/include/ode/common.h b/extern/ode/dist/include/ode/common.h deleted file mode 100644 index bd29d904028..00000000000 --- a/extern/ode/dist/include/ode/common.h +++ /dev/null @@ -1,307 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#ifndef _ODE_COMMON_H_ -#define _ODE_COMMON_H_ - -#include <ode/config.h> -#include <ode/error.h> - -#ifdef __cplusplus -extern "C" { -#endif - - -/* configuration stuff */ - -/* the efficient alignment. most platforms align data structures to some - * number of bytes, but this is not always the most efficient alignment. - * for example, many x86 compilers align to 4 bytes, but on a pentium it - * is important to align doubles to 8 byte boundaries (for speed), and - * the 4 floats in a SIMD register to 16 byte boundaries. many other - * platforms have similar behavior. setting a larger alignment can waste - * a (very) small amount of memory. NOTE: this number must be a power of - * two. this is set to 16 by default. - */ -#define EFFICIENT_ALIGNMENT 16 - - -/* constants */ - -/* pi and 1/sqrt(2) are defined here if necessary because they don't get - * defined in <math.h> on some platforms (like MS-Windows) - */ - -#ifndef M_PI -#define M_PI REAL(3.1415926535897932384626433832795029) -#endif -#ifndef M_SQRT1_2 -#define M_SQRT1_2 REAL(0.7071067811865475244008443621048490) -#endif - - -/* debugging: - * IASSERT is an internal assertion, i.e. a consistency check. if it fails - * we want to know where. - * UASSERT is a user assertion, i.e. if it fails a nice error message - * should be printed for the user. - * AASSERT is an arguments assertion, i.e. if it fails "bad argument(s)" - * is printed. - * DEBUGMSG just prints out a message - */ - -#ifndef dNODEBUG -#ifdef __GNUC__ -#define dIASSERT(a) if (!(a)) dDebug (d_ERR_IASSERT, \ - "assertion \"" #a "\" failed in %s() [%s]",__FUNCTION__,__FILE__); -#define dUASSERT(a,msg) if (!(a)) dDebug (d_ERR_UASSERT, \ - msg " in %s()", __FUNCTION__); -#define dDEBUGMSG(msg) dMessage (d_ERR_UASSERT, \ - msg " in %s()", __FUNCTION__); -#else -#define dIASSERT(a) if (!(a)) dDebug (d_ERR_IASSERT, \ - "assertion \"" #a "\" failed in %s:%d",__FILE__,__LINE__); -#define dUASSERT(a,msg) if (!(a)) dDebug (d_ERR_UASSERT, \ - msg " (%s:%d)", __FILE__,__LINE__); -#define dDEBUGMSG(msg) dMessage (d_ERR_UASSERT, \ - msg " (%s:%d)", __FILE__,__LINE__); -#endif -#else -#define dIASSERT(a) ; -#define dUASSERT(a,msg) ; -#define dDEBUGMSG(msg) ; -#endif -#define dAASSERT(a) dUASSERT(a,"Bad argument(s)") - -/* floating point data type, vector, matrix and quaternion types */ - -#if defined(dSINGLE) -typedef float dReal; -#elif defined(dDOUBLE) -typedef double dReal; -#else -#error You must #define dSINGLE or dDOUBLE -#endif - - -/* round an integer up to a multiple of 4, except that 0 and 1 are unmodified - * (used to compute matrix leading dimensions) - */ -#define dPAD(a) (((a) > 1) ? ((((a)-1)|3)+1) : (a)) - -/* these types are mainly just used in headers */ -typedef dReal dVector3[4]; -typedef dReal dVector4[4]; -typedef dReal dMatrix3[4*3]; -typedef dReal dMatrix4[4*4]; -typedef dReal dMatrix6[8*6]; -typedef dReal dQuaternion[4]; - - -/* precision dependent scalar math functions */ - -#if defined(dSINGLE) - -#define REAL(x) (x ## f) /* form a constant */ -#define dRecip(x) ((float)(1.0f/(x))) /* reciprocal */ -#define dSqrt(x) ((float)sqrt(x)) /* square root */ -#define dRecipSqrt(x) ((float)(1.0f/sqrt(x))) /* reciprocal square root */ -#define dSin(x) ((float)sin(x)) /* sine */ -#define dCos(x) ((float)cos(x)) /* cosine */ -#define dFabs(x) ((float)fabs(x)) /* absolute value */ -#define dAtan2(y,x) ((float)atan2((y),(x))) /* arc tangent with 2 args */ - -#elif defined(dDOUBLE) - -#define REAL(x) (x) -#define dRecip(x) (1.0/(x)) -#define dSqrt(x) sqrt(x) -#define dRecipSqrt(x) (1.0/sqrt(x)) -#define dSin(x) sin(x) -#define dCos(x) cos(x) -#define dFabs(x) fabs(x) -#define dAtan2(y,x) atan2((y),(x)) - -#else -#error You must #define dSINGLE or dDOUBLE -#endif - - -/* utility */ - - -/* round something up to be a multiple of the EFFICIENT_ALIGNMENT */ - -#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1) - - -/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste - * up to 15 bytes per allocation, depending on what alloca() returns. - */ - -#define dALLOCA16(n) \ - ((char*)dEFFICIENT_SIZE(((int)(alloca((n)+(EFFICIENT_ALIGNMENT-1)))))) - - -/* internal object types (all prefixed with `dx') */ - -struct dxWorld; /* dynamics world */ -struct dxSpace; /* collision space */ -struct dxBody; /* rigid body (dynamics object) */ -struct dxGeom; /* geometry (collision object) */ -struct dxJoint; -struct dxJointNode; -struct dxJointGroup; - -typedef struct dxWorld *dWorldID; -typedef struct dxSpace *dSpaceID; -typedef struct dxBody *dBodyID; -typedef struct dxGeom *dGeomID; -typedef struct dxJoint *dJointID; -typedef struct dxJointGroup *dJointGroupID; - - -/* error numbers */ - -enum { - d_ERR_UNKNOWN = 0, /* unknown error */ - d_ERR_IASSERT, /* internal assertion failed */ - d_ERR_UASSERT, /* user assertion failed */ - d_ERR_LCP /* user assertion failed */ -}; - - -/* joint type numbers */ - -enum { - dJointTypeNone = 0, /* or "unknown" */ - dJointTypeBall, - dJointTypeHinge, - dJointTypeSlider, - dJointTypeContact, - dJointTypeUniversal, - dJointTypeHinge2, - dJointTypeFixed, - dJointTypeNull, - dJointTypeAMotor -}; - - -/* an alternative way of setting joint parameters, using joint parameter - * structures and member constants. we don't actually do this yet. - */ - -/* -typedef struct dLimot { - int mode; - dReal lostop, histop; - dReal vel, fmax; - dReal fudge_factor; - dReal bounce, soft; - dReal suspension_erp, suspension_cfm; -} dLimot; - -enum { - dLimotLoStop = 0x0001, - dLimotHiStop = 0x0002, - dLimotVel = 0x0004, - dLimotFMax = 0x0008, - dLimotFudgeFactor = 0x0010, - dLimotBounce = 0x0020, - dLimotSoft = 0x0040 -}; -*/ - - -/* standard joint parameter names. why are these here? - because we don't want - * to include all the joint function definitions in joint.cpp. hmmmm. - * MSVC complains if we call D_ALL_PARAM_NAMES_X with a blank second argument, - * which is why we have the D_ALL_PARAM_NAMES macro as well. please copy and - * paste between these two. - */ - -#define D_ALL_PARAM_NAMES(start) \ - /* parameters for limits and motors */ \ - dParamLoStop = start, \ - dParamHiStop, \ - dParamVel, \ - dParamFMax, \ - dParamFudgeFactor, \ - dParamBounce, \ - dParamCFM, \ - dParamStopERP, \ - dParamStopCFM, \ - /* parameters for suspension */ \ - dParamSuspensionERP, \ - dParamSuspensionCFM, - -#define D_ALL_PARAM_NAMES_X(start,x) \ - /* parameters for limits and motors */ \ - dParamLoStop ## x = start, \ - dParamHiStop ## x, \ - dParamVel ## x, \ - dParamFMax ## x, \ - dParamFudgeFactor ## x, \ - dParamBounce ## x, \ - dParamCFM ## x, \ - dParamStopERP ## x, \ - dParamStopCFM ## x, \ - /* parameters for suspension */ \ - dParamSuspensionERP ## x, \ - dParamSuspensionCFM ## x, - -enum { - D_ALL_PARAM_NAMES(0) - D_ALL_PARAM_NAMES_X(0x100,2) - D_ALL_PARAM_NAMES_X(0x200,3) - - /* add a multiple of this constant to the basic parameter numbers to get - * the parameters for the second, third etc axes. - */ - dParamGroup=0x100 -}; - - -/* angular motor mode numbers */ - -enum{ - dAMotorUser = 0, - dAMotorEuler = 1 -}; - - -/* joint force feedback information */ - -typedef struct dJointFeedback { - dVector3 f1; // force applied to body 1 - dVector3 t1; // torque applied to body 1 - dVector3 f2; // force applied to body 2 - dVector3 t2; // torque applied to body 2 -} dJointFeedback; - - -#ifdef __cplusplus -} -#endif - -#endif - diff --git a/extern/ode/dist/include/ode/contact.h b/extern/ode/dist/include/ode/contact.h deleted file mode 100644 index 6fc66a179c0..00000000000 --- a/extern/ode/dist/include/ode/contact.h +++ /dev/null @@ -1,91 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#ifndef _ODE_CONTACT_H_ -#define _ODE_CONTACT_H_ - -#include <ode/common.h> - -#ifdef __cplusplus -extern "C" { -#endif - - -enum { - dContactMu2 = 0x001, - dContactFDir1 = 0x002, - dContactBounce = 0x004, - dContactSoftERP = 0x008, - dContactSoftCFM = 0x010, - dContactMotion1 = 0x020, - dContactMotion2 = 0x040, - dContactSlip1 = 0x080, - dContactSlip2 = 0x100, - - dContactApprox0 = 0x0000, - dContactApprox1_1 = 0x1000, - dContactApprox1_2 = 0x2000, - dContactApprox1 = 0x3000 -}; - - -typedef struct dSurfaceParameters { - /* must always be defined */ - int mode; - dReal mu; - - /* only defined if the corresponding flag is set in mode */ - dReal mu2; - dReal bounce; - dReal bounce_vel; - dReal soft_erp; - dReal soft_cfm; - dReal motion1,motion2; - dReal slip1,slip2; -} dSurfaceParameters; - - -/* contact info set by collision functions */ - -typedef struct dContactGeom { - dVector3 pos; - dVector3 normal; - dReal depth; - dGeomID g1,g2; -} dContactGeom; - - -/* contact info used by contact joint */ - -typedef struct dContact { - dSurfaceParameters surface; - dContactGeom geom; - dVector3 fdir1; -} dContact; - - -#ifdef __cplusplus -} -#endif - -#endif - diff --git a/extern/ode/dist/include/ode/error.h b/extern/ode/dist/include/ode/error.h deleted file mode 100644 index 1350bbc8427..00000000000 --- a/extern/ode/dist/include/ode/error.h +++ /dev/null @@ -1,64 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -/* this comes from the `reuse' library. copy any changes back to the source */ - -#ifndef _ODE_ERROR_H_ -#define _ODE_ERROR_H_ - -#include <ode/config.h> - -#ifdef __cplusplus -extern "C" { -#endif - -/* all user defined error functions have this type. error and debug functions - * should not return. - */ -typedef void dMessageFunction (int errnum, const char *msg, va_list ap); - -/* set a new error, debug or warning handler. if fn is 0, the default handlers - * are used. - */ -void dSetErrorHandler (dMessageFunction *fn); -void dSetDebugHandler (dMessageFunction *fn); -void dSetMessageHandler (dMessageFunction *fn); - -/* return the current error, debug or warning handler. if the return value is - * 0, the default handlers are in place. - */ -dMessageFunction *dGetErrorHandler(); -dMessageFunction *dGetDebugHandler(); -dMessageFunction *dGetMessageHandler(); - -/* generate a fatal error, debug trap or a message. */ -void dError (int num, const char *msg, ...); -void dDebug (int num, const char *msg, ...); -void dMessage (int num, const char *msg, ...); - - -#ifdef __cplusplus -} -#endif - -#endif - diff --git a/extern/ode/dist/include/ode/geom.h b/extern/ode/dist/include/ode/geom.h deleted file mode 100644 index 20fc8998ded..00000000000 --- a/extern/ode/dist/include/ode/geom.h +++ /dev/null @@ -1,153 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#ifndef _ODE_GEOM_H_ -#define _ODE_GEOM_H_ - -#include <ode/common.h> -#include <ode/space.h> -#include <ode/contact.h> - -#if defined SHARED_GEOM_H_INCLUDED_FROM_DEFINING_FILE -#define GLOBAL_SHAREDLIB_SPEC SHAREDLIBEXPORT -#else -#define GLOBAL_SHAREDLIB_SPEC SHAREDLIBIMPORT -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -/* ************************************************************************ */ -/* utility functions */ - -void dClosestLineSegmentPoints (const dVector3 a1, const dVector3 a2, - const dVector3 b1, const dVector3 b2, - dVector3 cp1, dVector3 cp2); - -int dBoxTouchesBox (const dVector3 _p1, const dMatrix3 R1, - const dVector3 side1, const dVector3 _p2, - const dMatrix3 R2, const dVector3 side2); - -void dInfiniteAABB (dGeomID geom, dReal aabb[6]); -void dCloseODE(); - -/* ************************************************************************ */ -/* standard classes */ - -/* class numbers */ -extern GLOBAL_SHAREDLIB_SPEC int dSphereClass; -extern GLOBAL_SHAREDLIB_SPEC int dBoxClass; -extern GLOBAL_SHAREDLIB_SPEC int dCCylinderClass; -extern GLOBAL_SHAREDLIB_SPEC int dPlaneClass; -extern GLOBAL_SHAREDLIB_SPEC int dGeomGroupClass; -extern GLOBAL_SHAREDLIB_SPEC int dGeomTransformClass; - -/* constructors */ -dGeomID dCreateSphere (dSpaceID space, dReal radius); -dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz); -dGeomID dCreatePlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d); -dGeomID dCreateCCylinder (dSpaceID space, dReal radius, dReal length); -dGeomID dCreateGeomGroup (dSpaceID space); - -/* set geometry parameters */ -void dGeomSphereSetRadius (dGeomID sphere, dReal radius); -void dGeomBoxSetLengths (dGeomID box, dReal lx, dReal ly, dReal lz); -void dGeomPlaneSetParams (dGeomID plane, dReal a, dReal b, dReal c, dReal d); -void dGeomCCylinderSetParams (dGeomID ccylinder, dReal radius, dReal length); - -/* get geometry parameters */ -int dGeomGetClass (dGeomID); -dReal dGeomSphereGetRadius (dGeomID sphere); -void dGeomBoxGetLengths (dGeomID box, dVector3 result); -void dGeomPlaneGetParams (dGeomID plane, dVector4 result); -void dGeomCCylinderGetParams (dGeomID ccylinder, - dReal *radius, dReal *length); - -/* general functions */ -void dGeomSetData (dGeomID, void *); -void *dGeomGetData (dGeomID); -void dGeomSetBody (dGeomID, dBodyID); -dBodyID dGeomGetBody (dGeomID); -void dGeomSetPosition (dGeomID, dReal x, dReal y, dReal z); -void dGeomSetRotation (dGeomID, const dMatrix3 R); -const dReal * dGeomGetPosition (dGeomID); -const dReal * dGeomGetRotation (dGeomID); -void dGeomDestroy (dGeomID); -void dGeomGetAABB (dGeomID, dReal aabb[6]); -dReal *dGeomGetSpaceAABB (dGeomID); - -/* ************************************************************************ */ -/* geometry group functions */ - -void dGeomGroupAdd (dGeomID group, dGeomID x); -void dGeomGroupRemove (dGeomID group, dGeomID x); -int dGeomGroupGetNumGeoms (dGeomID group); -dGeomID dGeomGroupGetGeom (dGeomID group, int i); - -/* ************************************************************************ */ -/* transformed geometry functions */ - -dGeomID dCreateGeomTransform (dSpaceID space); -void dGeomTransformSetGeom (dGeomID g, dGeomID obj); -dGeomID dGeomTransformGetGeom (dGeomID g); -void dGeomTransformSetCleanup (dGeomID g, int mode); -int dGeomTransformGetCleanup (dGeomID g); -void dGeomTransformSetInfo (dGeomID g, int mode); -int dGeomTransformGetInfo (dGeomID g); - -/* ************************************************************************ */ -/* general collision */ - -int dCollide (dGeomID o1, dGeomID o2, int flags, dContactGeom *contact, - int skip); - -/* ************************************************************************ */ -/* custom classes */ - -typedef void dGetAABBFn (dGeomID, dReal aabb[6]); -typedef int dColliderFn (dGeomID o1, dGeomID o2, - int flags, dContactGeom *contact, int skip); -typedef dColliderFn * dGetColliderFnFn (int num); -typedef void dGeomDtorFn (dGeomID o); -typedef int dAABBTestFn (dGeomID o1, dGeomID o2, dReal aabb[6]); - -typedef struct dGeomClass { - int bytes; - dGetColliderFnFn *collider; - dGetAABBFn *aabb; - dAABBTestFn *aabb_test; - dGeomDtorFn *dtor; -} dGeomClass; - -int dCreateGeomClass (const dGeomClass *classptr); -void * dGeomGetClassData (dGeomID); -dGeomID dCreateGeom (int classnum); - -/* ************************************************************************ */ - -#ifdef __cplusplus -} -#endif - -#endif - diff --git a/extern/ode/dist/include/ode/mass.h b/extern/ode/dist/include/ode/mass.h deleted file mode 100644 index 8e75da0c1eb..00000000000 --- a/extern/ode/dist/include/ode/mass.h +++ /dev/null @@ -1,98 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#ifndef _ODE_MASS_H_ -#define _ODE_MASS_H_ - -#include <ode/common.h> - -#ifdef __cplusplus -extern "C" { -#endif - -struct dMass; -typedef struct dMass dMass; - - -void dMassSetZero (dMass *); - -void dMassSetParameters (dMass *, dReal themass, - dReal cgx, dReal cgy, dReal cgz, - dReal I11, dReal I22, dReal I33, - dReal I12, dReal I13, dReal I23); - -void dMassSetSphere (dMass *, dReal density, dReal radius); - -void dMassSetCappedCylinder (dMass *, dReal density, int direction, - dReal a, dReal b); - -void dMassSetBox (dMass *, dReal density, - dReal lx, dReal ly, dReal lz); - -void dMassAdjust (dMass *, dReal newmass); - -void dMassTranslate (dMass *, dReal x, dReal y, dReal z); - -void dMassRotate (dMass *, const dMatrix3 R); - -void dMassAdd (dMass *a, const dMass *b); - - - -struct dMass { - dReal mass; - dVector4 c; - dMatrix3 I; - -#ifdef __cplusplus - dMass() - { dMassSetZero (this); } - void setZero() - { dMassSetZero (this); } - void setParameters (dReal themass, dReal cgx, dReal cgy, dReal cgz, - dReal I11, dReal I22, dReal I33, - dReal I12, dReal I13, dReal I23) - { dMassSetParameters (this,themass,cgx,cgy,cgz,I11,I22,I33,I12,I13,I23); } - void setSphere (dReal density, dReal radius) - { dMassSetSphere (this,density,radius); } - void setCappedCylinder (dReal density, int direction, dReal a, dReal b) - { dMassSetCappedCylinder (this,density,direction,a,b); } - void setBox (dReal density, dReal lx, dReal ly, dReal lz) - { dMassSetBox (this,density,lx,ly,lz); } - void adjust (dReal newmass) - { dMassAdjust (this,newmass); } - void translate (dReal x, dReal y, dReal z) - { dMassTranslate (this,x,y,z); } - void rotate (const dMatrix3 R) - { dMassRotate (this,R); } - void add (const dMass *b) - { dMassAdd (this,b); } -#endif -}; - - -#ifdef __cplusplus -} -#endif - -#endif - diff --git a/extern/ode/dist/include/ode/matrix.h b/extern/ode/dist/include/ode/matrix.h deleted file mode 100644 index 94d830095cd..00000000000 --- a/extern/ode/dist/include/ode/matrix.h +++ /dev/null @@ -1,195 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -/* optimized and unoptimized vector and matrix functions */ - -#ifndef _ODE_MATRIX_H_ -#define _ODE_MATRIX_H_ - -#include <ode/common.h> - - -#ifdef __cplusplus -extern "C" { -#endif - - -/* set a vector/matrix of size n to all zeros, or to a specific value. */ - -void dSetZero (dReal *a, int n); -void dSetValue (dReal *a, int n, dReal value); - - -/* get the dot product of two n*1 vectors. if n <= 0 then - * zero will be returned (in which case a and b need not be valid). - */ - -dReal dDot (const dReal *a, const dReal *b, int n); - - -/* get the dot products of (a0,b), (a1,b), etc and return them in outsum. - * all vectors are n*1. if n <= 0 then zeroes will be returned (in which case - * the input vectors need not be valid). this function is somewhat faster - * than calling dDot() for all of the combinations separately. - */ - -/* NOT INCLUDED in the library for now. -void dMultidot2 (const dReal *a0, const dReal *a1, - const dReal *b, dReal *outsum, int n); -*/ - - -/* matrix multiplication. all matrices are stored in standard row format. - * the digit refers to the argument that is transposed: - * 0: A = B * C (sizes: A:p*r B:p*q C:q*r) - * 1: A = B' * C (sizes: A:p*r B:q*p C:q*r) - * 2: A = B * C' (sizes: A:p*r B:p*q C:r*q) - * case 1,2 are equivalent to saying that the operation is A=B*C but - * B or C are stored in standard column format. - */ - -void dMultiply0 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r); -void dMultiply1 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r); -void dMultiply2 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r); - - -/* do an in-place cholesky decomposition on the lower triangle of the n*n - * symmetric matrix A (which is stored by rows). the resulting lower triangle - * will be such that L*L'=A. return 1 on success and 0 on failure (on failure - * the matrix is not positive definite). - */ - -int dFactorCholesky (dReal *A, int n); - - -/* solve for x: L*L'*x = b, and put the result back into x. - * L is size n*n, b is size n*1. only the lower triangle of L is considered. - */ - -void dSolveCholesky (const dReal *L, dReal *b, int n); - - -/* compute the inverse of the n*n positive definite matrix A and put it in - * Ainv. this is not especially fast. this returns 1 on success (A was - * positive definite) or 0 on failure (not PD). - */ - -int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n); - - -/* check whether an n*n matrix A is positive definite, return 1/0 (yes/no). - * positive definite means that x'*A*x > 0 for any x. this performs a - * cholesky decomposition of A. if the decomposition fails then the matrix - * is not positive definite. A is stored by rows. A is not altered. - */ - -int dIsPositiveDefinite (const dReal *A, int n); - - -/* factorize a matrix A into L*D*L', where L is lower triangular with ones on - * the diagonal, and D is diagonal. - * A is an n*n matrix stored by rows, with a leading dimension of n rounded - * up to 4. L is written into the strict lower triangle of A (the ones are not - * written) and the reciprocal of the diagonal elements of D are written into - * d. - */ -void dFactorLDLT (dReal *A, dReal *d, int n, int nskip); - - -/* solve L*x=b, where L is n*n lower triangular with ones on the diagonal, - * and x,b are n*1. b is overwritten with x. - * the leading dimension of L is `nskip'. - */ -void dSolveL1 (const dReal *L, dReal *b, int n, int nskip); - - -/* solve L'*x=b, where L is n*n lower triangular with ones on the diagonal, - * and x,b are n*1. b is overwritten with x. - * the leading dimension of L is `nskip'. - */ -void dSolveL1T (const dReal *L, dReal *b, int n, int nskip); - - -/* in matlab syntax: a(1:n) = a(1:n) .* d(1:n) */ - -void dVectorScale (dReal *a, const dReal *d, int n); - - -/* given `L', a n*n lower triangular matrix with ones on the diagonal, - * and `d', a n*1 vector of the reciprocal diagonal elements of an n*n matrix - * D, solve L*D*L'*x=b where x,b are n*1. x overwrites b. - * the leading dimension of L is `nskip'. - */ - -void dSolveLDLT (const dReal *L, const dReal *d, dReal *b, int n, int nskip); - - -/* given an L*D*L' factorization of an n*n matrix A, return the updated - * factorization L2*D2*L2' of A plus the following "top left" matrix: - * - * [ b a' ] <-- b is a[0] - * [ a 0 ] <-- a is a[1..n-1] - * - * - L has size n*n, its leading dimension is nskip. L is lower triangular - * with ones on the diagonal. only the lower triangle of L is referenced. - * - d has size n. d contains the reciprocal diagonal elements of D. - * - a has size n. - * the result is written into L, except that the left column of L and d[0] - * are not actually modified. see ldltaddTL.m for further comments. - */ -void dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip); - - -/* given an L*D*L' factorization of a permuted matrix A, produce a new - * factorization for row and column `r' removed. - * - A has size n1*n1, its leading dimension in nskip. A is symmetric and - * positive definite. only the lower triangle of A is referenced. - * A itself may actually be an array of row pointers. - * - L has size n2*n2, its leading dimension in nskip. L is lower triangular - * with ones on the diagonal. only the lower triangle of L is referenced. - * - d has size n2. d contains the reciprocal diagonal elements of D. - * - p is a permutation vector. it contains n2 indexes into A. each index - * must be in the range 0..n1-1. - * - r is the row/column of L to remove. - * the new L will be written within the old L, i.e. will have the same leading - * dimension. the last row and column of L, and the last element of d, are - * undefined on exit. - * - * a fast O(n^2) algorithm is used. see ldltremove.m for further comments. - */ -void dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d, - int n1, int n2, int r, int nskip); - - -/* given an n*n matrix A (with leading dimension nskip), remove the r'th row - * and column by moving elements. the new matrix will have the same leading - * dimension. the last row and column of A are untouched on exit. - */ -void dRemoveRowCol (dReal *A, int n, int nskip, int r); - - -#ifdef __cplusplus -} -#endif - -#endif - diff --git a/extern/ode/dist/include/ode/memory.h b/extern/ode/dist/include/ode/memory.h deleted file mode 100644 index 7ea668e4d39..00000000000 --- a/extern/ode/dist/include/ode/memory.h +++ /dev/null @@ -1,64 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -/* this comes from the `reuse' library. copy any changes back to the source */ - -#ifndef _ODE_MEMORY_H_ -#define _ODE_MEMORY_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -/* function types to allocate and free memory */ -typedef void * dAllocFunction (int size); -typedef void * dReallocFunction (void *ptr, int oldsize, int newsize); -typedef void dFreeFunction (void *ptr, int size); - -/* set new memory management functions. if fn is 0, the default handlers are - * used. */ -void dSetAllocHandler (dAllocFunction *fn); -void dSetReallocHandler (dReallocFunction *fn); -void dSetFreeHandler (dFreeFunction *fn); - -/* get current memory management functions */ -dAllocFunction *dGetAllocHandler (); -dReallocFunction *dGetReallocHandler (); -dFreeFunction *dGetFreeHandler (); - -/* allocate and free memory. */ -void * dAlloc (int size); -void * dRealloc (void *ptr, int oldsize, int newsize); -void dFree (void *ptr, int size); - -/* when alloc debugging is turned on, this indicates that the given block of - * alloc()ed memory should not be reported as "still in use" when the program - * exits. - */ -void dAllocDontReport (void *ptr); - -#ifdef __cplusplus -} -#endif - -#endif - diff --git a/extern/ode/dist/include/ode/misc.h b/extern/ode/dist/include/ode/misc.h deleted file mode 100644 index 241ad71ed9e..00000000000 --- a/extern/ode/dist/include/ode/misc.h +++ /dev/null @@ -1,86 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -/* miscellaneous math functions. these are mostly useful for testing */ - -#ifndef _ODE_MISC_H_ -#define _ODE_MISC_H_ - -#include <ode/common.h> - - -#ifdef __cplusplus -extern "C" { -#endif - - -/* return 1 if the random number generator is working. */ -int dTestRand(); - -/* return next 32 bit random number. this uses a not-very-random linear - * congruential method. - */ -unsigned long dRand(); - -/* get and set the current random number seed. */ -unsigned long dRandGetSeed(); -void dRandSetSeed (unsigned long s); - -/* return a random integer between 0..n-1. the distribution will get worse - * as n approaches 2^32. - */ -int dRandInt (int n); - -/* return a random real number between 0..1 */ -dReal dRandReal(); - -/* print out a matrix */ -#ifdef __cplusplus -void dPrintMatrix (dReal *A, int n, int m, char *fmt = "%10.4f ", - FILE *f=stdout); -#else -void dPrintMatrix (dReal *A, int n, int m, char *fmt, FILE *f); -#endif - -/* make a random vector with entries between +/- range. A has n elements. */ -void dMakeRandomVector (dReal *A, int n, dReal range); - -/* make a random matrix with entries between +/- range. A has size n*m. */ -void dMakeRandomMatrix (dReal *A, int n, int m, dReal range); - -/* clear the upper triangle of a square matrix */ -void dClearUpperTriangle (dReal *A, int n); - -/* return the maximum element difference between the two n*m matrices */ -dReal dMaxDifference (const dReal *A, const dReal *B, int n, int m); - -/* return the maximum element difference between the lower triangle of two - * n*n matrices */ -dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n); - - -#ifdef __cplusplus -} -#endif - -#endif - diff --git a/extern/ode/dist/include/ode/objects.h b/extern/ode/dist/include/ode/objects.h deleted file mode 100644 index 606051ead34..00000000000 --- a/extern/ode/dist/include/ode/objects.h +++ /dev/null @@ -1,202 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#ifndef _ODE_OBJECTS_H_ -#define _ODE_OBJECTS_H_ - -#include <ode/common.h> -#include <ode/mass.h> - -#ifdef __cplusplus -extern "C" { -#endif - -/* world */ - -dWorldID dWorldCreate(); -void dWorldDestroy (dWorldID); - -void dWorldSetGravity (dWorldID, dReal x, dReal y, dReal z); -void dWorldGetGravity (dWorldID, dVector3 gravity); -void dWorldSetERP (dWorldID, dReal erp); -dReal dWorldGetERP (dWorldID); -void dWorldSetCFM (dWorldID, dReal cfm); -dReal dWorldGetCFM (dWorldID); -void dWorldStep (dWorldID, dReal stepsize); -void dWorldImpulseToForce (dWorldID, dReal stepsize, - dReal ix, dReal iy, dReal iz, dVector3 force); - -/* bodies */ - -dBodyID dBodyCreate (dWorldID); -void dBodyDestroy (dBodyID); - -void dBodySetData (dBodyID, void *data); -void *dBodyGetData (dBodyID); - -void dBodySetPosition (dBodyID, dReal x, dReal y, dReal z); -void dBodySetRotation (dBodyID, const dMatrix3 R); -void dBodySetQuaternion (dBodyID, const dQuaternion q); -void dBodySetLinearVel (dBodyID, dReal x, dReal y, dReal z); -void dBodySetAngularVel (dBodyID, dReal x, dReal y, dReal z); -const dReal * dBodyGetPosition (dBodyID); -const dReal * dBodyGetRotation (dBodyID); /* ptr to 4x3 rot matrix */ -const dReal * dBodyGetQuaternion (dBodyID); -const dReal * dBodyGetLinearVel (dBodyID); -const dReal * dBodyGetAngularVel (dBodyID); - -void dBodySetMass (dBodyID, const dMass *mass); -void dBodyGetMass (dBodyID, dMass *mass); - -void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz); -void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz); -void dBodyAddRelForce (dBodyID, dReal fx, dReal fy, dReal fz); -void dBodyAddRelTorque (dBodyID, dReal fx, dReal fy, dReal fz); -void dBodyAddForceAtPos (dBodyID, dReal fx, dReal fy, dReal fz, - dReal px, dReal py, dReal pz); -void dBodyAddForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz, - dReal px, dReal py, dReal pz); -void dBodyAddRelForceAtPos (dBodyID, dReal fx, dReal fy, dReal fz, - dReal px, dReal py, dReal pz); -void dBodyAddRelForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz, - dReal px, dReal py, dReal pz); - -const dReal * dBodyGetForce (dBodyID); -const dReal * dBodyGetTorque (dBodyID); -void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z); -void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z); - -void dBodyGetRelPointPos (dBodyID, dReal px, dReal py, dReal pz, - dVector3 result); -void dBodyGetRelPointVel (dBodyID, dReal px, dReal py, dReal pz, - dVector3 result); -void dBodyGetPointVel (dBodyID, dReal px, dReal py, dReal pz, - dVector3 result); -void dBodyGetPosRelPoint (dBodyID, dReal px, dReal py, dReal pz, - dVector3 result); -void dBodyVectorToWorld (dBodyID, dReal px, dReal py, dReal pz, - dVector3 result); -void dBodyVectorFromWorld (dBodyID, dReal px, dReal py, dReal pz, - dVector3 result); - -void dBodySetFiniteRotationMode (dBodyID, int mode); -void dBodySetFiniteRotationAxis (dBodyID, dReal x, dReal y, dReal z); - -int dBodyGetFiniteRotationMode (dBodyID); -void dBodyGetFiniteRotationAxis (dBodyID, dVector3 result); - -int dBodyGetNumJoints (dBodyID b); -dJointID dBodyGetJoint (dBodyID, int index); - -void dBodyEnable (dBodyID); -void dBodyDisable (dBodyID); -int dBodyIsEnabled (dBodyID); - -void dBodySetGravityMode (dBodyID b, int mode); -int dBodyGetGravityMode (dBodyID b); - - -/* joints */ - -dJointID dJointCreateBall (dWorldID, dJointGroupID); -dJointID dJointCreateHinge (dWorldID, dJointGroupID); -dJointID dJointCreateSlider (dWorldID, dJointGroupID); -dJointID dJointCreateContact (dWorldID, dJointGroupID, const dContact *); -dJointID dJointCreateHinge2 (dWorldID, dJointGroupID); -dJointID dJointCreateUniversal (dWorldID, dJointGroupID); -dJointID dJointCreateFixed (dWorldID, dJointGroupID); -dJointID dJointCreateNull (dWorldID, dJointGroupID); -dJointID dJointCreateAMotor (dWorldID, dJointGroupID); - -void dJointDestroy (dJointID); - -dJointGroupID dJointGroupCreate (int max_size); -void dJointGroupDestroy (dJointGroupID); -void dJointGroupEmpty (dJointGroupID); - -void dJointAttach (dJointID, dBodyID body1, dBodyID body2); -void dJointSetData (dJointID, void *data); -void *dJointGetData (dJointID); -int dJointGetType (dJointID); -dBodyID dJointGetBody (dJointID, int index); - -void dJointSetFeedback (dJointID, dJointFeedback *); -dJointFeedback *dJointGetFeedback (dJointID); - -void dJointSetBallAnchor (dJointID, dReal x, dReal y, dReal z); -void dJointSetHingeAnchor (dJointID, dReal x, dReal y, dReal z); -void dJointSetHingeAxis (dJointID, dReal x, dReal y, dReal z); -void dJointSetHingeParam (dJointID, int parameter, dReal value); -void dJointSetSliderAxis (dJointID, dReal x, dReal y, dReal z); -void dJointSetSliderParam (dJointID, int parameter, dReal value); -void dJointSetHinge2Anchor (dJointID, dReal x, dReal y, dReal z); -void dJointSetHinge2Axis1 (dJointID, dReal x, dReal y, dReal z); -void dJointSetHinge2Axis2 (dJointID, dReal x, dReal y, dReal z); -void dJointSetHinge2Param (dJointID, int parameter, dReal value); -void dJointSetUniversalAnchor (dJointID, dReal x, dReal y, dReal z); -void dJointSetUniversalAxis1 (dJointID, dReal x, dReal y, dReal z); -void dJointSetUniversalAxis2 (dJointID, dReal x, dReal y, dReal z); -void dJointSetFixed (dJointID); -void dJointSetAMotorNumAxes (dJointID, int num); -void dJointSetAMotorAxis (dJointID, int anum, int rel, - dReal x, dReal y, dReal z); -void dJointSetAMotorAngle (dJointID, int anum, dReal angle); -void dJointSetAMotorParam (dJointID, int parameter, dReal value); -void dJointSetAMotorMode (dJointID, int mode); - -void dJointGetBallAnchor (dJointID, dVector3 result); -void dJointGetHingeAnchor (dJointID, dVector3 result); -void dJointGetHingeAxis (dJointID, dVector3 result); -dReal dJointGetHingeParam (dJointID, int parameter); -dReal dJointGetHingeAngle (dJointID); -dReal dJointGetHingeAngleRate (dJointID); -dReal dJointGetSliderPosition (dJointID); -dReal dJointGetSliderPositionRate (dJointID); -void dJointGetSliderAxis (dJointID, dVector3 result); -dReal dJointGetSliderParam (dJointID, int parameter); -void dJointGetHinge2Anchor (dJointID, dVector3 result); -void dJointGetHinge2Axis1 (dJointID, dVector3 result); -void dJointGetHinge2Axis2 (dJointID, dVector3 result); -dReal dJointGetHinge2Param (dJointID, int parameter); -dReal dJointGetHinge2Angle1 (dJointID); -dReal dJointGetHinge2Angle1Rate (dJointID); -dReal dJointGetHinge2Angle2Rate (dJointID); -void dJointGetUniversalAnchor (dJointID, dVector3 result); -void dJointGetUniversalAxis1 (dJointID, dVector3 result); -void dJointGetUniversalAxis2 (dJointID, dVector3 result); -int dJointGetAMotorNumAxes (dJointID); -void dJointGetAMotorAxis (dJointID, int anum, dVector3 result); -int dJointGetAMotorAxisRel (dJointID, int anum); -dReal dJointGetAMotorAngle (dJointID, int anum); -dReal dJointGetAMotorAngleRate (dJointID, int anum); -dReal dJointGetAMotorParam (dJointID, int parameter); -int dJointGetAMotorMode (dJointID); - -int dAreConnected (dBodyID, dBodyID); - - -#ifdef __cplusplus -} -#endif - -#endif - diff --git a/extern/ode/dist/include/ode/ode.h b/extern/ode/dist/include/ode/ode.h deleted file mode 100644 index da7f2e1a0e7..00000000000 --- a/extern/ode/dist/include/ode/ode.h +++ /dev/null @@ -1,45 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#ifndef _ODE_ODE_H_ -#define _ODE_ODE_H_ - -/* include *everything* here */ - -#include <ode/config.h> -#include <ode/common.h> -#include <ode/contact.h> -#include <ode/error.h> -#include <ode/memory.h> -#include <ode/odemath.h> -#include <ode/matrix.h> -#include <ode/timer.h> -#include <ode/rotation.h> -#include <ode/mass.h> -#include <ode/space.h> -#include <ode/geom.h> -#include <ode/misc.h> -#include <ode/objects.h> -#include <ode/odecpp.h> - -#endif - diff --git a/extern/ode/dist/include/ode/odecpp.h b/extern/ode/dist/include/ode/odecpp.h deleted file mode 100644 index 35712236d1d..00000000000 --- a/extern/ode/dist/include/ode/odecpp.h +++ /dev/null @@ -1,797 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -// C++ interface for everything - - -#ifndef _ODE_ODECPP_H_ -#define _ODE_ODECPP_H_ -#ifdef __cplusplus - -#include <ode/error.h> - - -class dWorld { - dWorldID _id; - - // intentionally undefined, don't use these - dWorld (const dWorld &); - void operator= (const dWorld &); - -public: - dWorld() - { _id = dWorldCreate(); } - ~dWorld() - { dWorldDestroy (_id); } - - dWorldID id() const - { return _id; } - operator dWorldID() const - { return _id; } - - void setGravity (dReal x, dReal y, dReal z) - { dWorldSetGravity (_id,x,y,z); } - void getGravity (dVector3 g) const - { dWorldGetGravity (_id,g); } - - void setERP (dReal erp) - { dWorldSetERP(_id, erp); } - dReal getERP() const - { return dWorldGetERP(_id); } - - void setCFM (dReal cfm) - { dWorldSetCFM(_id, cfm); } - dReal getCFM() const - { return dWorldGetCFM(_id); } - - void step (dReal stepsize) - { dWorldStep (_id,stepsize); } - - void impulseToForce (dReal stepsize, dReal ix, dReal iy, dReal iz, - dVector3 force) - { dWorldImpulseToForce (_id,stepsize,ix,iy,iz,force); } -}; - - -class dBody { - dBodyID _id; - - // intentionally undefined, don't use these - dBody (const dBody &); - void operator= (const dBody &); - -public: - dBody() - { _id = 0; } - dBody (dWorldID world) - { _id = dBodyCreate (world); } - ~dBody() - { if (_id) dBodyDestroy (_id); } - - void create (dWorldID world) { - if (_id) dBodyDestroy (_id); - _id = dBodyCreate (world); - } - - dBodyID id() const - { return _id; } - operator dBodyID() const - { return _id; } - - void setData (void *data) - { dBodySetData (_id,data); } - void *getData() const - { return dBodyGetData (_id); } - - void setPosition (dReal x, dReal y, dReal z) - { dBodySetPosition (_id,x,y,z); } - void setRotation (const dMatrix3 R) - { dBodySetRotation (_id,R); } - void setQuaternion (const dQuaternion q) - { dBodySetQuaternion (_id,q); } - void setLinearVel (dReal x, dReal y, dReal z) - { dBodySetLinearVel (_id,x,y,z); } - void setAngularVel (dReal x, dReal y, dReal z) - { dBodySetAngularVel (_id,x,y,z); } - - const dReal * getPosition() const - { return dBodyGetPosition (_id); } - const dReal * getRotation() const - { return dBodyGetRotation (_id); } - const dReal * getQuaternion() const - { return dBodyGetQuaternion (_id); } - const dReal * getLinearVel() const - { return dBodyGetLinearVel (_id); } - const dReal * getAngularVel() const - { return dBodyGetAngularVel (_id); } - - void setMass (const dMass *mass) - { dBodySetMass (_id,mass); } - void getMass (dMass *mass) const - { dBodyGetMass (_id,mass); } - - void addForce (dReal fx, dReal fy, dReal fz) - { dBodyAddForce (_id, fx, fy, fz); } - void addTorque (dReal fx, dReal fy, dReal fz) - { dBodyAddTorque (_id, fx, fy, fz); } - void addRelForce (dReal fx, dReal fy, dReal fz) - { dBodyAddRelForce (_id, fx, fy, fz); } - void addRelTorque (dReal fx, dReal fy, dReal fz) - { dBodyAddRelTorque (_id, fx, fy, fz); } - void addForceAtPos (dReal fx, dReal fy, dReal fz, - dReal px, dReal py, dReal pz) - { dBodyAddForceAtPos (_id, fx, fy, fz, px, py, pz); } - void addForceAtRelPos (dReal fx, dReal fy, dReal fz, - dReal px, dReal py, dReal pz) - { dBodyAddForceAtRelPos (_id, fx, fy, fz, px, py, pz); } - void addRelForceAtPos (dReal fx, dReal fy, dReal fz, - dReal px, dReal py, dReal pz) - { dBodyAddRelForceAtPos (_id, fx, fy, fz, px, py, pz); } - void addRelForceAtRelPos (dReal fx, dReal fy, dReal fz, - dReal px, dReal py, dReal pz) - { dBodyAddRelForceAtRelPos (_id, fx, fy, fz, px, py, pz); } - - const dReal * getForce() const - { return dBodyGetForce(_id); } - const dReal * getTorque() const - { return dBodyGetTorque(_id); } - void setForce (dReal x, dReal y, dReal z) - { dBodySetForce (_id,x,y,z); } - void setTorque (dReal x, dReal y, dReal z) - { dBodySetTorque (_id,x,y,z); } - - void enable() - { dBodyEnable (_id); } - void disable() - { dBodyDisable (_id); } - int isEnabled() const - { return dBodyIsEnabled (_id); } - - void getRelPointPos (dReal px, dReal py, dReal pz, dVector3 result) const - { dBodyGetRelPointPos (_id, px, py, pz, result); } - void getRelPointVel (dReal px, dReal py, dReal pz, dVector3 result) const - { dBodyGetRelPointVel (_id, px, py, pz, result); } - void getPointVel (dReal px, dReal py, dReal pz, dVector3 result) const - { dBodyGetPointVel (_id,px,py,pz,result); } - void getPosRelPoint (dReal px, dReal py, dReal pz, dVector3 result) const - { dBodyGetPosRelPoint (_id,px,py,pz,result); } - void vectorToWorld (dReal px, dReal py, dReal pz, dVector3 result) const - { dBodyVectorToWorld (_id,px,py,pz,result); } - void vectorFromWorld (dReal px, dReal py, dReal pz, dVector3 result) const - { dBodyVectorFromWorld (_id,px,py,pz,result); } - - void setFiniteRotationMode (int mode) - { dBodySetFiniteRotationMode (_id, mode); } - void setFiniteRotationAxis (dReal x, dReal y, dReal z) - { dBodySetFiniteRotationAxis (_id, x, y, z); } - - int getFiniteRotationMode() const - { return dBodyGetFiniteRotationMode (_id); } - void getFiniteRotationAxis (dVector3 result) const - { dBodyGetFiniteRotationAxis (_id, result); } - - int getNumJoints() const - { return dBodyGetNumJoints (_id); } - dJointID getJoint (int index) const - { return dBodyGetJoint (_id, index); } - - void setGravityMode (int mode) - { dBodySetGravityMode (_id,mode); } - int getGravityMode() const - { return dBodyGetGravityMode (_id); } - - int isConnectedTo (dBodyID body) const - { return dAreConnected (_id, body); } -}; - - -class dJointGroup { - dJointGroupID _id; - - // intentionally undefined, don't use these - dJointGroup (const dJointGroup &); - void operator= (const dJointGroup &); - -public: - dJointGroup (int dummy_arg=0) - { _id = dJointGroupCreate (0); } - ~dJointGroup() - { dJointGroupDestroy (_id); } - void create (int dummy_arg=0) { - if (_id) dJointGroupDestroy (_id); - _id = dJointGroupCreate (0); - } - - dJointGroupID id() const - { return _id; } - operator dJointGroupID() const - { return _id; } - - void empty() - { dJointGroupEmpty (_id); } -}; - - -class dJoint { -private: - // intentionally undefined, don't use these - dJoint (const dJoint &) ; - void operator= (const dJoint &); - -protected: - dJointID _id; - -public: - dJoint() - { _id = 0; } - ~dJoint() - { if (_id) dJointDestroy (_id); } - - dJointID id() const - { return _id; } - operator dJointID() const - { return _id; } - - void attach (dBodyID body1, dBodyID body2) - { dJointAttach (_id, body1, body2); } - - void setData (void *data) - { dJointSetData (_id, data); } - void *getData (void *data) const - { return dJointGetData (_id); } - - int getType() const - { return dJointGetType (_id); } - - dBodyID getBody (int index) const - { return dJointGetBody (_id, index); } -}; - - -class dBallJoint : public dJoint { -private: - // intentionally undefined, don't use these - dBallJoint (const dBallJoint &); - void operator= (const dBallJoint &); - -public: - dBallJoint() { } - dBallJoint (dWorldID world, dJointGroupID group=0) - { _id = dJointCreateBall (world, group); } - - void create (dWorldID world, dJointGroupID group=0) { - if (_id) dJointDestroy (_id); - _id = dJointCreateBall (world, group); - } - - void setAnchor (dReal x, dReal y, dReal z) - { dJointSetBallAnchor (_id, x, y, z); } - void getAnchor (dVector3 result) const - { dJointGetBallAnchor (_id, result); } -} ; - - -class dHingeJoint : public dJoint { - // intentionally undefined, don't use these - dHingeJoint (const dHingeJoint &); - void operator = (const dHingeJoint &); - -public: - dHingeJoint() { } - dHingeJoint (dWorldID world, dJointGroupID group=0) - { _id = dJointCreateHinge (world, group); } - - void create (dWorldID world, dJointGroupID group=0) { - if (_id) dJointDestroy (_id); - _id = dJointCreateHinge (world, group); - } - - void setAnchor (dReal x, dReal y, dReal z) - { dJointSetHingeAnchor (_id, x, y, z); } - void getAnchor (dVector3 result) const - { dJointGetHingeAnchor (_id, result); } - - void setAxis (dReal x, dReal y, dReal z) - { dJointSetHingeAxis (_id, x, y, z); } - void getAxis (dVector3 result) const - { dJointGetHingeAxis (_id, result); } - - dReal getAngle() const - { return dJointGetHingeAngle (_id); } - dReal getAngleRate() const - { return dJointGetHingeAngleRate (_id); } - - void setParam (int parameter, dReal value) - { dJointSetHingeParam (_id, parameter, value); } - dReal getParam (int parameter) const - { return dJointGetHingeParam (_id, parameter); } -}; - - -class dSliderJoint : public dJoint { - // intentionally undefined, don't use these - dSliderJoint (const dSliderJoint &); - void operator = (const dSliderJoint &); - -public: - dSliderJoint() { } - dSliderJoint (dWorldID world, dJointGroupID group=0) - { _id = dJointCreateSlider (world, group); } - - void create (dWorldID world, dJointGroupID group=0) { - if (_id) dJointDestroy (_id); - _id = dJointCreateSlider (world, group); - } - - void setAxis (dReal x, dReal y, dReal z) - { dJointSetSliderAxis (_id, x, y, z); } - void getAxis (dVector3 result) const - { dJointGetSliderAxis (_id, result); } - - dReal getPosition() const - { return dJointGetSliderPosition (_id); } - dReal getPositionRate() const - { return dJointGetSliderPositionRate (_id); } - - void setParam (int parameter, dReal value) - { dJointSetSliderParam (_id, parameter, value); } - dReal getParam (int parameter) const - { return dJointGetSliderParam (_id, parameter); } -}; - - -class dUniversalJoint : public dJoint { - // intentionally undefined, don't use these - dUniversalJoint (const dUniversalJoint &); - void operator = (const dUniversalJoint &); - -public: - dUniversalJoint() { } - dUniversalJoint (dWorldID world, dJointGroupID group=0) - { _id = dJointCreateUniversal (world, group); } - - void create (dWorldID world, dJointGroupID group=0) { - if (_id) dJointDestroy (_id); - _id = dJointCreateUniversal (world, group); - } - - void setAnchor (dReal x, dReal y, dReal z) - { dJointSetUniversalAnchor (_id, x, y, z); } - void setAxis1 (dReal x, dReal y, dReal z) - { dJointSetUniversalAxis1 (_id, x, y, z); } - void setAxis2 (dReal x, dReal y, dReal z) - { dJointSetUniversalAxis2 (_id, x, y, z); } - - void getAnchor (dVector3 result) const - { dJointGetUniversalAnchor (_id, result); } - void getAxis1 (dVector3 result) const - { dJointGetUniversalAxis1 (_id, result); } - void getAxis2 (dVector3 result) const - { dJointGetUniversalAxis2 (_id, result); } -}; - - -class dHinge2Joint : public dJoint { - // intentionally undefined, don't use these - dHinge2Joint (const dHinge2Joint &); - void operator = (const dHinge2Joint &); - -public: - dHinge2Joint() { } - dHinge2Joint (dWorldID world, dJointGroupID group=0) - { _id = dJointCreateHinge2 (world, group); } - - void create (dWorldID world, dJointGroupID group=0) { - if (_id) dJointDestroy (_id); - _id = dJointCreateHinge2 (world, group); - } - - void setAnchor (dReal x, dReal y, dReal z) - { dJointSetHinge2Anchor (_id, x, y, z); } - void setAxis1 (dReal x, dReal y, dReal z) - { dJointSetHinge2Axis1 (_id, x, y, z); } - void setAxis2 (dReal x, dReal y, dReal z) - { dJointSetHinge2Axis2 (_id, x, y, z); } - - void getAnchor (dVector3 result) const - { dJointGetHinge2Anchor (_id, result); } - void getAxis1 (dVector3 result) const - { dJointGetHinge2Axis1 (_id, result); } - void getAxis2 (dVector3 result) const - { dJointGetHinge2Axis2 (_id, result); } - - dReal getAngle1() const - { return dJointGetHinge2Angle1 (_id); } - dReal getAngle1Rate() const - { return dJointGetHinge2Angle1Rate (_id); } - dReal getAngle2Rate() const - { return dJointGetHinge2Angle2Rate (_id); } - - void setParam (int parameter, dReal value) - { dJointSetHinge2Param (_id, parameter, value); } - dReal getParam (int parameter) const - { return dJointGetHinge2Param (_id, parameter); } -}; - - -class dFixedJoint : public dJoint { - // intentionally undefined, don't use these - dFixedJoint (const dFixedJoint &); - void operator = (const dFixedJoint &); - -public: - dFixedJoint() { } - dFixedJoint (dWorldID world, dJointGroupID group=0) - { _id = dJointCreateFixed (world, group); } - - void create (dWorldID world, dJointGroupID group=0) { - if (_id) dJointDestroy (_id); - _id = dJointCreateFixed (world, group); - } - - void set() - { dJointSetFixed (_id); } -}; - - -class dContactJoint : public dJoint { - // intentionally undefined, don't use these - dContactJoint (const dContactJoint &); - void operator = (const dContactJoint &); - -public: - dContactJoint() { } - dContactJoint (dWorldID world, dJointGroupID group, dContact *contact) - { _id = dJointCreateContact (world, group, contact); } - - void create (dWorldID world, dJointGroupID group, dContact *contact) { - if (_id) dJointDestroy (_id); - _id = dJointCreateContact (world, group, contact); - } -}; - - -class dNullJoint : public dJoint { - // intentionally undefined, don't use these - dNullJoint (const dNullJoint &); - void operator = (const dNullJoint &); - -public: - dNullJoint() { } - dNullJoint (dWorldID world, dJointGroupID group=0) - { _id = dJointCreateNull (world, group); } - - void create (dWorldID world, dJointGroupID group=0) { - if (_id) dJointDestroy (_id); - _id = dJointCreateNull (world, group); - } -}; - - -class dAMotorJoint : public dJoint { - // intentionally undefined, don't use these - dAMotorJoint (const dAMotorJoint &); - void operator = (const dAMotorJoint &); - -public: - dAMotorJoint() { } - dAMotorJoint (dWorldID world, dJointGroupID group=0) - { _id = dJointCreateAMotor (world, group); } - - void create (dWorldID world, dJointGroupID group=0) { - if (_id) dJointDestroy (_id); - _id = dJointCreateAMotor (world, group); - } - - void setMode (int mode) - { dJointSetAMotorMode (_id, mode); } - int getMode() const - { return dJointGetAMotorMode (_id); } - - void setNumAxes (int num) - { dJointSetAMotorNumAxes (_id, num); } - int getNumAxes() const - { return dJointGetAMotorNumAxes (_id); } - - void setAxis (int anum, int rel, dReal x, dReal y, dReal z) - { dJointSetAMotorAxis (_id, anum, rel, x, y, z); } - void getAxis (int anum, dVector3 result) const - { dJointGetAMotorAxis (_id, anum, result); } - int getAxisRel (int anum) const - { return dJointGetAMotorAxisRel (_id, anum); } - - void setAngle (int anum, dReal angle) - { dJointSetAMotorAngle (_id, anum, angle); } - dReal getAngle (int anum) const - { return dJointGetAMotorAngle (_id, anum); } - dReal getAngleRate (int anum) - { return dJointGetAMotorAngleRate (_id,anum); } - - void setParam (int parameter, dReal value) - { dJointSetAMotorParam (_id, parameter, value); } - dReal getParam (int parameter) const - { return dJointGetAMotorParam (_id, parameter); } -}; - - -class dGeom { - // intentionally undefined, don't use these - dGeom (dGeom &); - void operator= (dGeom &); - -protected: - dGeomID _id; - -public: - dGeom() - { _id = 0; } - ~dGeom() - { if (_id) dGeomDestroy (_id); } - - dGeomID id() const - { return _id; } - operator dGeomID() const - { return _id; } - - void destroy() { - if (_id) dGeomDestroy (_id); - _id = 0; - } - - int getClass() const - { return dGeomGetClass (_id); } - - void setData (void *data) - { dGeomSetData (_id,data); } - void *getData() const - { return dGeomGetData (_id); } - - void setBody (dBodyID b) - { dGeomSetBody (_id,b); } - dBodyID getBody() const - { return dGeomGetBody (_id); } - - void setPosition (dReal x, dReal y, dReal z) - { dGeomSetPosition (_id,x,y,z); } - const dReal * getPosition() const - { return dGeomGetPosition (_id); } - - void setRotation (const dMatrix3 R) - { dGeomSetRotation (_id,R); } - const dReal * getRotation() const - { return dGeomGetRotation (_id); } - - void getAABB (dReal aabb[6]) const - { dGeomGetAABB (_id, aabb); } - const dReal *getSpaceAABB() const - { return dGeomGetSpaceAABB (_id); } -}; - - -class dSpace { - // intentionally undefined, don't use these - dSpace (dSpace &); - void operator= (dSpace &); - -protected: - dSpaceID _id; - - // the default constructor is protected so that you - // can't instance this class. you must instance one - // of its subclasses instead. - dSpace () { _id = 0; } - -public: - ~dSpace() - { dSpaceDestroy (_id); } - - dSpaceID id() const - { return _id; } - operator dSpaceID() const - { return _id; } - - void add (dGeomID x) - { dSpaceAdd (_id, x); } - void remove (dGeomID x) - { dSpaceRemove (_id, x); } - int query (dGeomID x) - { return dSpaceQuery (_id,x); } - - void collide (void *data, dNearCallback *callback) - { dSpaceCollide (_id,data,callback); } -}; - - -class dSimpleSpace : public dSpace { - // intentionally undefined, don't use these - dSimpleSpace (dSimpleSpace &); - void operator= (dSimpleSpace &); - -public: - dSimpleSpace () - { _id = dSimpleSpaceCreate(); } -}; - - -class dHashSpace : public dSpace { - // intentionally undefined, don't use these - dHashSpace (dHashSpace &); - void operator= (dHashSpace &); - -public: - dHashSpace () - { _id = dHashSpaceCreate(); } - void setLevels (int minlevel, int maxlevel) - { dHashSpaceSetLevels (_id,minlevel,maxlevel); } -}; - - -class dSphere : public dGeom { - // intentionally undefined, don't use these - dSphere (dSphere &); - void operator= (dSphere &); - -public: - dSphere () { } - dSphere (dSpaceID space, dReal radius) - { _id = dCreateSphere (space, radius); } - - void create (dSpaceID space, dReal radius) { - if (_id) dGeomDestroy (_id); - _id = dCreateSphere (space, radius); - } - - void setRadius (dReal radius) - { dGeomSphereSetRadius (_id, radius); } - dReal getRadius() const - { return dGeomSphereGetRadius (_id); } -}; - - -class dBox : public dGeom { - // intentionally undefined, don't use these - dBox (dBox &); - void operator= (dBox &); - -public: - dBox () { } - dBox (dSpaceID space, dReal lx, dReal ly, dReal lz) - { _id = dCreateBox (space,lx,ly,lz); } - - void create (dSpaceID space, dReal lx, dReal ly, dReal lz) { - if (_id) dGeomDestroy (_id); - _id = dCreateBox (space,lx,ly,lz); - } - - void setLengths (dReal lx, dReal ly, dReal lz) - { dGeomBoxSetLengths (_id, lx, ly, lz); } - void getLengths (dVector3 result) const - { dGeomBoxGetLengths (_id,result); } -}; - - -class dPlane : public dGeom { - // intentionally undefined, don't use these - dPlane (dPlane &); - void operator= (dPlane &); - -public: - dPlane() { } - dPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d) - { _id = dCreatePlane (space,a,b,c,d); } - - void create (dSpaceID space, dReal a, dReal b, dReal c, dReal d) { - if (_id) dGeomDestroy (_id); - _id = dCreatePlane (space,a,b,c,d); - } - - void setParams (dReal a, dReal b, dReal c, dReal d) - { dGeomPlaneSetParams (_id, a, b, c, d); } - void getParams (dVector4 result) const - { dGeomPlaneGetParams (_id,result); } -}; - - -class dCCylinder : public dGeom { - // intentionally undefined, don't use these - dCCylinder (dCCylinder &); - void operator= (dCCylinder &); - -public: - dCCylinder() { } - dCCylinder (dSpaceID space, dReal radius, dReal length) - { _id = dCreateCCylinder (space,radius,length); } - - void create (dSpaceID space, dReal radius, dReal length) { - if (_id) dGeomDestroy (_id); - _id = dCreateCCylinder (space,radius,length); - } - - void setParams (dReal radius, dReal length) - { dGeomCCylinderSetParams (_id, radius, length); } - void getParams (dReal *radius, dReal *length) const - { dGeomCCylinderGetParams (_id,radius,length); } -}; - - -class dGeomGroup : public dGeom { - // intentionally undefined, don't use these - dGeomGroup (dGeomGroup &); - void operator= (dGeomGroup &); - -public: - dGeomGroup() { } - dGeomGroup (dSpaceID space) - { _id = dCreateGeomGroup (space); } - - void create (dSpaceID space=0) { - if (_id) dGeomDestroy (_id); - _id = dCreateGeomGroup (space); - } - - void add (dGeomID x) - { dGeomGroupAdd (_id, x); } - void remove (dGeomID x) - { dGeomGroupRemove (_id, x); } - - int getNumGeoms() const - { return dGeomGroupGetNumGeoms (_id); } - dGeomID getGeom (int i) const - { return dGeomGroupGetGeom (_id, i); } -}; - - -class dGeomTransform : public dGeom { - // intentionally undefined, don't use these - dGeomTransform (dGeomTransform &); - void operator= (dGeomTransform &); - -public: - dGeomTransform() { } - dGeomTransform (dSpaceID space) - { _id = dCreateGeomTransform (space); } - - void create (dSpaceID space=0) { - if (_id) dGeomDestroy (_id); - _id = dCreateGeomTransform (space); - } - - void setGeom (dGeomID geom) - { dGeomTransformSetGeom (_id, geom); } - dGeomID getGeom() const - { return dGeomTransformGetGeom (_id); } - - void setCleanup (int mode) - { dGeomTransformSetCleanup (_id,mode); } - int getCleanup (dGeomID g) - { return dGeomTransformGetCleanup (_id); } - - void setInfo (int mode) - { dGeomTransformSetInfo (_id,mode); } - int getInfo() - { return dGeomTransformGetInfo (_id); } -}; - -#endif - -#endif - diff --git a/extern/ode/dist/include/ode/odecpp_old.h b/extern/ode/dist/include/ode/odecpp_old.h deleted file mode 100644 index b7b55232f89..00000000000 --- a/extern/ode/dist/include/ode/odecpp_old.h +++ /dev/null @@ -1,317 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -/* this is the old C++ interface, the new C++ interface is not quite - * compatible with this. but this file is kept around in case you were - * using the old interface. - */ - -#ifndef _ODE_ODECPP_H_ -#define _ODE_ODECPP_H_ -#ifdef __cplusplus - -#include <ode/error.h> - - -class dWorld { - dWorldID _id; - - dWorld (dWorld &) { dDebug (0,"bad"); } - void operator= (dWorld &) { dDebug (0,"bad"); } - -public: - dWorld() - { _id = dWorldCreate(); } - ~dWorld() - { dWorldDestroy (_id); } - dWorldID id() - { return _id; } - - void setGravity (dReal x, dReal y, dReal z) - { dWorldSetGravity (_id,x,y,z); } - void getGravity (dVector3 g) - { dWorldGetGravity (_id,g); } - void step (dReal stepsize) - { dWorldStep (_id,stepsize); } -}; - - -class dBody { - dBodyID _id; - - dBody (dBody &) { dDebug (0,"bad"); } - void operator= (dBody &) { dDebug (0,"bad"); } - -public: - dBody() - { _id = 0; } - dBody (dWorld &world) - { _id = dBodyCreate (world.id()); } - ~dBody() - { dBodyDestroy (_id); } - void create (dWorld &world) - { if (_id) dBodyDestroy (_id); _id = dBodyCreate (world.id()); } - dBodyID id() - { return _id; } - - void setData (void *data) - { dBodySetData (_id,data); } - void *getData() - { return dBodyGetData (_id); } - - void setPosition (dReal x, dReal y, dReal z) - { dBodySetPosition (_id,x,y,z); } - void setRotation (const dMatrix3 R) - { dBodySetRotation (_id,R); } - void setQuaternion (const dQuaternion q) - { dBodySetQuaternion (_id,q); } - void setLinearVel (dReal x, dReal y, dReal z) - { dBodySetLinearVel (_id,x,y,z); } - void setAngularVel (dReal x, dReal y, dReal z) - { dBodySetAngularVel (_id,x,y,z); } - - const dReal * getPosition() - { return dBodyGetPosition (_id); } - const dReal * getRotation() - { return dBodyGetRotation (_id); } - const dReal * getQuaternion() - { return dBodyGetQuaternion (_id); } - const dReal * getLinearVel() - { return dBodyGetLinearVel (_id); } - const dReal * getAngularVel() - { return dBodyGetAngularVel (_id); } - - void setMass (const dMass *mass) - { dBodySetMass (_id,mass); } - void getMass (dMass *mass) - { dBodyGetMass (_id,mass); } - - void addForce (dReal fx, dReal fy, dReal fz) - { dBodyAddForce (_id, fx, fy, fz); } - void addTorque (dReal fx, dReal fy, dReal fz) - { dBodyAddTorque (_id, fx, fy, fz); } - void addRelForce (dReal fx, dReal fy, dReal fz) - { dBodyAddRelForce (_id, fx, fy, fz); } - void addRelTorque (dReal fx, dReal fy, dReal fz) - { dBodyAddRelTorque (_id, fx, fy, fz); } - void addForceAtPos (dReal fx, dReal fy, dReal fz, - dReal px, dReal py, dReal pz) - { dBodyAddForceAtPos (_id, fx, fy, fz, px, py, pz); } - void addRelForceAtPos (dReal fx, dReal fy, dReal fz, - dReal px, dReal py, dReal pz) - { dBodyAddRelForceAtPos (_id, fx, fy, fz, px, py, pz); } - void addRelForceAtRelPos (dReal fx, dReal fy, dReal fz, - dReal px, dReal py, dReal pz) - { dBodyAddRelForceAtRelPos (_id, fx, fy, fz, px, py, pz); } - - void getRelPointPos (dReal px, dReal py, dReal pz, dVector3 result) - { dBodyGetRelPointPos (_id, px, py, pz, result); } - void getRelPointVel (dReal px, dReal py, dReal pz, dVector3 result) - { dBodyGetRelPointVel (_id, px, py, pz, result); } - - int isConnectedTo (const dBody &b) - { return dAreConnected (_id,b._id); } -}; - - -class dJointGroup { - dJointGroupID _id; - - dJointGroup (dJointGroup &) { dDebug (0,"bad"); } - void operator= (dJointGroup &) { dDebug (0,"bad"); } - -public: - dJointGroup() - { _id = 0; } - dJointGroup (int max_size) - { _id = dJointGroupCreate (max_size); } - ~dJointGroup() - { dJointGroupDestroy (_id); } - void create (int max_size) - { if (_id) dJointGroupDestroy (_id); _id = dJointGroupCreate (max_size); } - dJointGroupID id() - { return _id; } - - void empty() - { dJointGroupEmpty (_id); } -}; - - -class dJoint { - dJointID _id; - - dJoint (dJoint &) { dDebug (0,"bad"); } - void operator= (dJoint &) { dDebug (0,"bad"); } - -public: - dJoint() - { _id = 0; } - ~dJoint() - { dJointDestroy (_id); } - dJointID id() - { return _id; } - - void createBall (dWorld &world, dJointGroup *group=0) { - if (_id) dJointDestroy (_id); - _id = dJointCreateBall (world.id(), group ? group->id() : 0); - } - void createHinge (dWorld &world, dJointGroup *group=0) { - if (_id) dJointDestroy (_id); - _id = dJointCreateHinge (world.id(), group ? group->id() : 0); - } - void createSlider (dWorld &world, dJointGroup *group=0) { - if (_id) dJointDestroy (_id); - _id = dJointCreateSlider (world.id(), group ? group->id() : 0); - } - void createContact (dWorld &world, dJointGroup *group, dContact *contact) { - if (_id) dJointDestroy (_id); - _id = dJointCreateContact (world.id(), group ? group->id() : 0, contact); - } - - void attach (dBody &body1, dBody &body2) - { dJointAttach (_id, body1.id(), body2.id()); } - - void setBallAnchor (dReal x, dReal y, dReal z) - { dJointSetBallAnchor (_id, x, y, z); } - void setHingeAnchor (dReal x, dReal y, dReal z) - { dJointSetHingeAnchor (_id, x, y, z); } - - void setHingeAxis (dReal x, dReal y, dReal z) - { dJointSetHingeAxis (_id, x, y, z); } - void setSliderAxis (dReal x, dReal y, dReal z) - { dJointSetSliderAxis (_id, x, y, z); } - - void getBallAnchor (dVector3 result) - { dJointGetBallAnchor (_id, result); } - void getHingeAnchor (dVector3 result) - { dJointGetHingeAnchor (_id, result); } - - void getHingeAxis (dVector3 result) - { dJointGetHingeAxis (_id, result); } - void getSliderAxis (dVector3 result) - { dJointGetSliderAxis (_id, result); } -}; - - -class dSpace { - dSpaceID _id; - - dSpace (dSpace &) { dDebug (0,"bad"); } - void operator= (dSpace &) { dDebug (0,"bad"); } - -public: - dSpace () - { _id = dHashSpaceCreate(); } - ~dSpace() - { dSpaceDestroy (_id); } - dSpaceID id() - { return _id; } - void collide (void *data, dNearCallback *callback) - { dSpaceCollide (_id,data,callback); } -}; - - -class dGeom { - dGeomID _id; - - dGeom (dGeom &) { dDebug (0,"bad"); } - void operator= (dGeom &) { dDebug (0,"bad"); } - -public: - dGeom() - { _id = 0; } - ~dGeom() - { dGeomDestroy (_id); } - dGeomID id() - { return _id; } - - void createSphere (dSpace &space, dReal radius) { - if (_id) dGeomDestroy (_id); - _id = dCreateSphere (space.id(),radius); - } - - void createBox (dSpace &space, dReal lx, dReal ly, dReal lz) { - if (_id) dGeomDestroy (_id); - _id = dCreateBox (space.id(),lx,ly,lz); - } - - void createPlane (dSpace &space, dReal a, dReal b, dReal c, dReal d) { - if (_id) dGeomDestroy (_id); - _id = dCreatePlane (space.id(),a,b,c,d); - } - - void createCCylinder (dSpace &space, dReal radius, dReal length) { - if (_id) dGeomDestroy (_id); - _id = dCreateCCylinder (space.id(),radius,length); - } - - void destroy() { - if (_id) dGeomDestroy (_id); - _id = 0; - } - - int getClass() - { return dGeomGetClass (_id); } - - dReal sphereGetRadius() - { return dGeomSphereGetRadius (_id); } - - void boxGetLengths (dVector3 result) - { dGeomBoxGetLengths (_id,result); } - - void planeGetParams (dVector4 result) - { dGeomPlaneGetParams (_id,result); } - - void CCylinderGetParams (dReal *radius, dReal *length) - { dGeomCCylinderGetParams (_id,radius,length); } - - void setData (void *data) - { dGeomSetData (_id,data); } - - void *getData() - { return dGeomGetData (_id); } - - void setBody (dBody &b) - { dGeomSetBody (_id,b.id()); } - void setBody (dBodyID b) - { dGeomSetBody (_id,b); } - - dBodyID getBody() - { return dGeomGetBody (_id); } - - void setPosition (dReal x, dReal y, dReal z) - { dGeomSetPosition (_id,x,y,z); } - - void setRotation (const dMatrix3 R) - { dGeomSetRotation (_id,R); } - - const dReal * getPosition() - { return dGeomGetPosition (_id); } - - const dReal * getRotation() - { return dGeomGetRotation (_id); } -}; - -#endif - -#endif - diff --git a/extern/ode/dist/include/ode/odemath.h b/extern/ode/dist/include/ode/odemath.h deleted file mode 100644 index 5f41a27b5a1..00000000000 --- a/extern/ode/dist/include/ode/odemath.h +++ /dev/null @@ -1,217 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#ifndef _ODE_ODEMATH_H_ -#define _ODE_ODEMATH_H_ - -#include <ode/common.h> - -#ifdef __cplusplus -extern "C" { -#endif - - -/* 3-way dot product. dDOTpq means that elements of `a' and `b' are spaced - * p and q indexes apart respectively. dDOT() means dDOT11. - */ - -#ifdef __cplusplus -inline dReal dDOT (const dReal *a, const dReal *b) - { return ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2]); } -inline dReal dDOT14(const dReal *a, const dReal *b) - { return ((a)[0]*(b)[0] + (a)[1]*(b)[4] + (a)[2]*(b)[8]); } -inline dReal dDOT41(const dReal *a, const dReal *b) - { return ((a)[0]*(b)[0] + (a)[4]*(b)[1] + (a)[8]*(b)[2]); } -inline dReal dDOT44(const dReal *a, const dReal *b) - { return ((a)[0]*(b)[0] + (a)[4]*(b)[4] + (a)[8]*(b)[8]); } -#else -#define dDOT(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2]) -#define dDOT14(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[4] + (a)[2]*(b)[8]) -#define dDOT41(a,b) ((a)[0]*(b)[0] + (a)[4]*(b)[1] + (a)[8]*(b)[2]) -#define dDOT44(a,b) ((a)[0]*(b)[0] + (a)[4]*(b)[4] + (a)[8]*(b)[8]) -#endif - - -/* cross product, set a = b x c. dCROSSpqr means that elements of `a', `b' - * and `c' are spaced p, q and r indexes apart respectively. - * dCROSS() means dCROSS111. `op' is normally `=', but you can set it to - * +=, -= etc to get other effects. - */ - -#define dCROSS(a,op,b,c) \ - (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \ - (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \ - (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]); -#define dCROSSpqr(a,op,b,c,p,q,r) \ - (a)[ 0] op ((b)[ q]*(c)[2*r] - (b)[2*q]*(c)[ r]); \ - (a)[ p] op ((b)[2*q]*(c)[ 0] - (b)[ 0]*(c)[2*r]); \ - (a)[2*p] op ((b)[ 0]*(c)[ r] - (b)[ q]*(c)[ 0]); -#define dCROSS114(a,op,b,c) dCROSSpqr(a,op,b,c,1,1,4) -#define dCROSS141(a,op,b,c) dCROSSpqr(a,op,b,c,1,4,1) -#define dCROSS144(a,op,b,c) dCROSSpqr(a,op,b,c,1,4,4) -#define dCROSS411(a,op,b,c) dCROSSpqr(a,op,b,c,4,1,1) -#define dCROSS414(a,op,b,c) dCROSSpqr(a,op,b,c,4,1,4) -#define dCROSS441(a,op,b,c) dCROSSpqr(a,op,b,c,4,4,1) -#define dCROSS444(a,op,b,c) dCROSSpqr(a,op,b,c,4,4,4) - - -/* set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b. - * A is stored by rows, and has `skip' elements per row. the matrix is - * assumed to be already zero, so this does not write zero elements! - * if (plus,minus) is (+,-) then a positive version will be written. - * if (plus,minus) is (-,+) then a negative version will be written. - */ - -#define dCROSSMAT(A,a,skip,plus,minus) \ - (A)[1] = minus (a)[2]; \ - (A)[2] = plus (a)[1]; \ - (A)[(skip)+0] = plus (a)[2]; \ - (A)[(skip)+2] = minus (a)[0]; \ - (A)[2*(skip)+0] = minus (a)[1]; \ - (A)[2*(skip)+1] = plus (a)[0]; - - -/* compute the distance between two 3-vectors (oops, C++!) */ -#ifdef __cplusplus -inline dReal dDISTANCE (const dVector3 a, const dVector3 b) - { return dSqrt( (a[0]-b[0])*(a[0]-b[0]) + (a[1]-b[1])*(a[1]-b[1]) + - (a[2]-b[2])*(a[2]-b[2]) ); } -#else -#define dDISTANCE(a,b) \ - (dSqrt( ((a)[0]-(b)[0])*((a)[0]-(b)[0]) + ((a)[1]-(b)[1])*((a)[1]-(b)[1]) + \ - ((a)[2]-(b)[2])*((a)[2]-(b)[2]) )) -#endif - - -/* normalize 3x1 and 4x1 vectors (i.e. scale them to unit length) */ -void dNormalize3 (dVector3 a); -void dNormalize4 (dVector4 a); - - -/* given a unit length "normal" vector n, generate vectors p and q vectors - * that are an orthonormal basis for the plane space perpendicular to n. - * i.e. this makes p,q such that n,p,q are all perpendicular to each other. - * q will equal n x p. if n is not unit length then p will be unit length but - * q wont be. - */ - -void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q); - - -/* special case matrix multipication, with operator selection */ - -#define dMULTIPLYOP0_331(A,op,B,C) \ - (A)[0] op dDOT((B),(C)); \ - (A)[1] op dDOT((B+4),(C)); \ - (A)[2] op dDOT((B+8),(C)); -#define dMULTIPLYOP1_331(A,op,B,C) \ - (A)[0] op dDOT41((B),(C)); \ - (A)[1] op dDOT41((B+1),(C)); \ - (A)[2] op dDOT41((B+2),(C)); -#define dMULTIPLYOP0_133(A,op,B,C) \ - (A)[0] op dDOT14((B),(C)); \ - (A)[1] op dDOT14((B),(C+1)); \ - (A)[2] op dDOT14((B),(C+2)); -#define dMULTIPLYOP0_333(A,op,B,C) \ - (A)[0] op dDOT14((B),(C)); \ - (A)[1] op dDOT14((B),(C+1)); \ - (A)[2] op dDOT14((B),(C+2)); \ - (A)[4] op dDOT14((B+4),(C)); \ - (A)[5] op dDOT14((B+4),(C+1)); \ - (A)[6] op dDOT14((B+4),(C+2)); \ - (A)[8] op dDOT14((B+8),(C)); \ - (A)[9] op dDOT14((B+8),(C+1)); \ - (A)[10] op dDOT14((B+8),(C+2)); -#define dMULTIPLYOP1_333(A,op,B,C) \ - (A)[0] op dDOT44((B),(C)); \ - (A)[1] op dDOT44((B),(C+1)); \ - (A)[2] op dDOT44((B),(C+2)); \ - (A)[4] op dDOT44((B+1),(C)); \ - (A)[5] op dDOT44((B+1),(C+1)); \ - (A)[6] op dDOT44((B+1),(C+2)); \ - (A)[8] op dDOT44((B+2),(C)); \ - (A)[9] op dDOT44((B+2),(C+1)); \ - (A)[10] op dDOT44((B+2),(C+2)); -#define dMULTIPLYOP2_333(A,op,B,C) \ - (A)[0] op dDOT((B),(C)); \ - (A)[1] op dDOT((B),(C+4)); \ - (A)[2] op dDOT((B),(C+8)); \ - (A)[4] op dDOT((B+4),(C)); \ - (A)[5] op dDOT((B+4),(C+4)); \ - (A)[6] op dDOT((B+4),(C+8)); \ - (A)[8] op dDOT((B+8),(C)); \ - (A)[9] op dDOT((B+8),(C+4)); \ - (A)[10] op dDOT((B+8),(C+8)); - -#ifdef __cplusplus - -inline void dMULTIPLY0_331(dReal *A, const dReal *B, const dReal *C) - { dMULTIPLYOP0_331(A,=,B,C) } -inline void dMULTIPLY1_331(dReal *A, const dReal *B, const dReal *C) - { dMULTIPLYOP1_331(A,=,B,C) } -inline void dMULTIPLY0_133(dReal *A, const dReal *B, const dReal *C) - { dMULTIPLYOP0_133(A,=,B,C) } -inline void dMULTIPLY0_333(dReal *A, const dReal *B, const dReal *C) - { dMULTIPLYOP0_333(A,=,B,C) } -inline void dMULTIPLY1_333(dReal *A, const dReal *B, const dReal *C) - { dMULTIPLYOP1_333(A,=,B,C) } -inline void dMULTIPLY2_333(dReal *A, const dReal *B, const dReal *C) - { dMULTIPLYOP2_333(A,=,B,C) } - -inline void dMULTIPLYADD0_331(dReal *A, const dReal *B, const dReal *C) - { dMULTIPLYOP0_331(A,+=,B,C) } -inline void dMULTIPLYADD1_331(dReal *A, const dReal *B, const dReal *C) - { dMULTIPLYOP1_331(A,+=,B,C) } -inline void dMULTIPLYADD0_133(dReal *A, const dReal *B, const dReal *C) - { dMULTIPLYOP0_133(A,+=,B,C) } -inline void dMULTIPLYADD0_333(dReal *A, const dReal *B, const dReal *C) - { dMULTIPLYOP0_333(A,+=,B,C) } -inline void dMULTIPLYADD1_333(dReal *A, const dReal *B, const dReal *C) - { dMULTIPLYOP1_333(A,+=,B,C) } -inline void dMULTIPLYADD2_333(dReal *A, const dReal *B, const dReal *C) - { dMULTIPLYOP2_333(A,+=,B,C) } - -#else - -#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) -#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C) -#define dMULTIPLY0_133(A,B,C) dMULTIPLYOP0_133(A,=,B,C) -#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C) -#define dMULTIPLY1_333(A,B,C) dMULTIPLYOP1_333(A,=,B,C) -#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C) - -#define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C) -#define dMULTIPLYADD1_331(A,B,C) dMULTIPLYOP1_331(A,+=,B,C) -#define dMULTIPLYADD0_133(A,B,C) dMULTIPLYOP0_133(A,+=,B,C) -#define dMULTIPLYADD0_333(A,B,C) dMULTIPLYOP0_333(A,+=,B,C) -#define dMULTIPLYADD1_333(A,B,C) dMULTIPLYOP1_333(A,+=,B,C) -#define dMULTIPLYADD2_333(A,B,C) dMULTIPLYOP2_333(A,+=,B,C) - -#endif - - -#ifdef __cplusplus -} -#endif - -#endif - diff --git a/extern/ode/dist/include/ode/rotation.h b/extern/ode/dist/include/ode/rotation.h deleted file mode 100644 index 19204c651f3..00000000000 --- a/extern/ode/dist/include/ode/rotation.h +++ /dev/null @@ -1,65 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#ifndef _ODE_ROTATION_H_ -#define _ODE_ROTATION_H_ - -#include <ode/common.h> - -#ifdef __cplusplus -extern "C" { -#endif - - -void dRSetIdentity (dMatrix3 R); - -void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az, - dReal angle); - -void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi); - -void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az, - dReal bx, dReal by, dReal bz); - -void dQSetIdentity (dQuaternion q); - -void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az, - dReal angle); - -void dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc); -void dQMultiply1 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc); -void dQMultiply2 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc); -void dQMultiply3 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc); - -void dQtoR (const dQuaternion q, dMatrix3 R); - -void dRtoQ (const dMatrix3 R, dQuaternion q); - -void dWtoDQ (const dVector3 w, const dQuaternion q, dVector4 dq); - - -#ifdef __cplusplus -} -#endif - -#endif - diff --git a/extern/ode/dist/include/ode/space.h b/extern/ode/dist/include/ode/space.h deleted file mode 100644 index c540cd69d6e..00000000000 --- a/extern/ode/dist/include/ode/space.h +++ /dev/null @@ -1,78 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#ifndef _ODE_SPACE_H_ -#define _ODE_SPACE_H_ - -#include <ode/common.h> - -#ifdef __cplusplus -extern "C" { -#endif - -struct dContactGeom; - -typedef void dNearCallback (void *data, dGeomID o1, dGeomID o2); - - -/* extra information the space needs in every geometry object */ - -typedef struct dGeomSpaceData { - dGeomID next; -} dGeomSpaceData; - - -dSpaceID dSimpleSpaceCreate(); -dSpaceID dHashSpaceCreate(); - -void dSpaceDestroy (dSpaceID); -void dSpaceAdd (dSpaceID, dGeomID); -void dSpaceRemove (dSpaceID, dGeomID); -void dSpaceCollide (dSpaceID space, void *data, dNearCallback *callback); -int dSpaceQuery (dSpaceID, dGeomID); - -void dHashSpaceSetLevels (dSpaceID space, int minlevel, int maxlevel); - - -/* @@@ NOT FLEXIBLE ENOUGH - * - * generate contacts for those objects in the space that touch each other. - * an array of contacts is created on the alternative stack using - * StackAlloc(), and a pointer to the array is returned. the size of the - * array is returned by the function. - */ -/* int dSpaceCollide (dSpaceID space, dContactGeom **contact_array); */ - - -/* HMMMMM... i dont think so. - * tell the space that an object has moved, so its representation in the - * space should be changed. - */ -/* void dSpaceObjectMoved (dSpaceID, dGeomID); */ - - -#ifdef __cplusplus -} -#endif - -#endif - diff --git a/extern/ode/dist/include/ode/timer.h b/extern/ode/dist/include/ode/timer.h deleted file mode 100644 index fe2574feb3c..00000000000 --- a/extern/ode/dist/include/ode/timer.h +++ /dev/null @@ -1,77 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#ifndef _ODE_TIMER_H_ -#define _ODE_TIMER_H_ - -#include <ode/config.h> - -#ifdef __cplusplus -extern "C" { -#endif - - -/* stop watch objects */ - -typedef struct dStopwatch { - double time; /* total clock count */ - unsigned long cc[2]; /* clock count since last `start' */ -} dStopwatch; - -void dStopwatchReset (dStopwatch *); -void dStopwatchStart (dStopwatch *); -void dStopwatchStop (dStopwatch *); -double dStopwatchTime (dStopwatch *); /* returns total time in secs */ - - -/* code timers */ - -void dTimerStart (const char *description); /* pass a static string here */ -void dTimerNow (const char *description); /* pass a static string here */ -void dTimerEnd(); - -/* print out a timer report. if `average' is nonzero, print out the average - * time for each slot (this is only meaningful if the same start-now-end - * calls are being made repeatedly. - */ -void dTimerReport (FILE *fout, int average); - - -/* resolution */ - -/* returns the timer ticks per second implied by the timing hardware or API. - * the actual timer resolution may not be this great. - */ -double dTimerTicksPerSecond(); - -/* returns an estimate of the actual timer resolution, in seconds. this may - * be greater than 1/ticks_per_second. - */ -double dTimerResolution(); - - -#ifdef __cplusplus -} -#endif - -#endif - diff --git a/extern/ode/dist/ode/README b/extern/ode/dist/ode/README deleted file mode 100644 index dd4596f9935..00000000000 --- a/extern/ode/dist/ode/README +++ /dev/null @@ -1,158 +0,0 @@ -Dynamics Library. -================= - -CONVENTIONS ------------ - -matrix storage --------------- - -matrix operations like factorization are expensive, so we must store the data -in a way that is most useful to the matrix code. we want the ability to update -the dynamics library without recompiling applications, e.g. so users can take -advantage of new floating point hardware. so we must settle on a single -format. because of the prevalence of 4-way SIMD, the format is this: store -the matrix by rows or columns, and each column is rounded up to a multiple of -4 elements. the extra "padding" elements at the end of each row/column are set -to 0. this is called the "standard format". to indicate if the data is stored -by rows or columns, we will say "standard row format" or "standard column -format". hopefully this decision will remain good in the future, as more and -more processors have 4-way SIMD, and 3D graphics always needs fast 4x4 -matrices. - -exception: matrices that have only one column or row (vectors), are always -stored as consecutive elements in standard row format, i.e. there is no -interior padding, only padding at the end. - -thus: all 3x1 floating point vectors are stored as 4x1 vectors: (x,x,x,0). -also: all 6x1 spatial velocities and accelerations are split into 3x1 position - and angular components, which are stored as contiguous 4x1 vectors. - -ALL matrices are stored by in standard row format. - - -arguments ---------- - -3x1 vector arguments to set() functions are supplied as x,y,z. -3x1 vector result arguments to get() function are pointers to arrays. -larger vectors are always supplied and returned as pointers. -all coordinates are in the global frame except where otherwise specified. -output-only arguments are usually supplied at the end. - - -memory allocation ------------------ - -with many C/C++ libraries memory allocation is a difficult problem to solve. -who allocates the memory? who frees it? must objects go on the heap or can -they go on the stack or in static storage? to provide the maximum flexibility, -the dynamics and collision libraries do not do their own memory allocation. -you must pass in pointers to externally allocated chunks of the right sizes. -the body, joint and colllision object structures are all exported, so you -can make instances of those structure and pass pointers to them. - -there are helper functions which allocate objects out of areans, in case you -need loots of dynamic creation and deletion. - -BUT!!! this ties us down to the body/joint/collision representation. - -a better approach is to supply custom memory allocation functions -(e.g. dlAlloc() etc). - - -C versus C++ ... ? ------------------- - -everything should be C linkable, and there should be C header files for -everything. but we want to develop in C++. so do this: - * all comments are "//". automatically convert to /**/ for distribution. - * structures derived from other structures --> automatically convert? - - -WORLDS ------- - -might want better terminology here. - -the dynamics world (DWorld) is a list of systems. each system corresponds to -one or more bodies, or perhaps some other kinds of physical object. -each system corresponds to one or more objects in the collision world -(there does not have to be a one-to-one correspondence between bodies and -collision objects). - -systems are simulated separately, perhaps using completely different -techniques. we must do something special when systems collide. -systems collide when collision objects belonging to system A touch -collision objects belonging to system B. - -for each collision point, the system must provide matrix equation data -that is used to compute collision forces. once those forces are computed, -the system must incorporate the forces into its timestep. -PROBLEM: what if we intertwine the LCP problems of the two systems - then -this simple approach wont work. - -the dynamics world contains two kinds of objects: bodies and joints. -joints connect two bodies together. - -the world contains one of more partitions. each partition is a collection of -bodies and joints such that each body is attached (through one or more joints) -to every other body. - -Joints ------- - -a joint can be connected to one or two bodies. -if the joint is only connected to one body, joint.node[1].body == 0. -joint.node[0].body is always valid. - - -Linkage -------- - -this library will always be statically linked with the app, for these reasons: - * collision space is selected at compile time, it adds data to the geom - objects. - - -Optimization ------------- - -doubles must be aligned on 8 byte boundaries! - - -MinGW on Windows issues ------------------------ - -* the .rc file for drawstuff needs a different include, try winresrc.h. - -* it seems we can't have both main() and WinMain() without the entry point - defaulting to main() and having resource loading problems. this screws up - what i was trying to do in the drawstuff library. perhaps main2() ? - -* remember to compile resources to COFF format RES files. - - - -Collision ---------- - -to plug in your own collision handling, replace (some of?) these functions -with your own. collision should be a separate library that you can link in -or not. your own library can call components in this collision library, e.g. -if you want polymorphic spaces instead of a single statically called space. - -creating an object will automatically register the appropriate -class (if necessary). how can we ensure that the minimum amount of code is -linked in? e.g. only one space handler, and sphere-sphere and sphere-box and -box-box collision code (if spheres and boxes instanced). - -the user creates a collision space, and for each dynamics object that is -created a collision object is inserted into the space. the collision -object's pos and R pointers are set to the corresponding dynamics -variables. - -there should be utility functions which create the dynamics and collision -objects at the same time, e.g. dMakeSphere(). - -collision objects and dynamics objects keep pointers to each other. diff --git a/extern/ode/dist/ode/fbuild/BuildDot b/extern/ode/dist/ode/fbuild/BuildDot deleted file mode 100644 index 09b49274da8..00000000000 --- a/extern/ode/dist/ode/fbuild/BuildDot +++ /dev/null @@ -1,148 +0,0 @@ -#!/usr/bin/perl -# -######################################################################### -# # -# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. # -# All rights reserved. Email: russ@q12.org Web: www.q12.org # -# # -# This library is free software; you can redistribute it and/or # -# modify it under the terms of EITHER: # -# (1) The GNU Lesser General Public License as published by the Free # -# Software Foundation; either version 2.1 of the License, or (at # -# your option) any later version. The text of the GNU Lesser # -# General Public License is included with this library in the # -# file LICENSE.TXT. # -# (2) The BSD-style license that is included with this library in # -# the file LICENSE-BSD.TXT. # -# # -# This library is distributed in the hope that it will be useful, # -# but WITHOUT ANY WARRANTY; without even the implied warranty of # -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files # -# LICENSE.TXT and LICENSE-BSD.TXT for more details. # -# # -######################################################################### - -# dot product code generator. -# -# code generation parameters, set in a parameters file: -# FNAME : name of source file to generate - a .c file will be made -# UNROLL1 : inner loop unrolling factor (1..) -# FETCH : max num of a[i]'s and b[i]'s to load ahead of muls -# LAT1 : load -> mul latency (>=1) -# LAT2 : mul -> add latency (>=1). if this is 1, use fused mul-add -# -############################################################################# - -require ("BuildUtil"); - -# get and check code generation parameters -error ("Usage: BuildDot <parameters-file>") if $#ARGV != 0; -do $ARGV[0]; - -if (!defined($FNAME) || !defined($UNROLL1) || !defined($FETCH) || - !defined($LAT1) || !defined($LAT2)) { - error ("code generation parameters not defined"); -} - -# check parameters -error ("bad UNROLL1") if $UNROLL1 < 1; -error ("bad FETCH") if $FETCH < 1; -error ("bad LAT1") if $LAT1 < 1; -error ("bad LAT2") if $LAT2 < 1; - -############################################################################# - -open (FOUT,">$FNAME.c") or die "can't open $FNAME.c for writing"; - -# file and function header -output (<<END); -/* generated code, do not edit. */ - -#include "ode/matrix.h" - - -dReal dDot (const dReal *a, const dReal *b, int n) -{ -END - -output ("dReal "); -for ($i=0; $i<$UNROLL1; $i++) { - output ("p$i,q$i,m$i,"); -} -output ("sum;\n"); - -output (<<END); -sum = 0; -n -= $UNROLL1; -while (n >= 0) { -END - -@load = (); # slot where a[i]'s and b[i]'s loaded -@mul = (); # slot where multiply i happened -@add = (); # slow where add i happened - -# in the future we may want to reduce the number of variables declared, -# so these arrays will be useful. -@pqused = (); # 1 if p/q[i] loaded with data, 0 once that data's used -@mused = (); # 1 if m[i] loaded with data, 0 once that data's used -@pqmap = (); # map virtual p/q variables to actual p/q variables -@mmap = (); # map virtual m variables to actual m variables - -output ("p0 = a[0]; q0 = b[0];\n"); -push (@load,0); - -$slot=0; # one slot for every load/mul/add/nop issued -for (;;) { - $startslot = $slot; - - # do next load - if (($#load - $#mul) < $FETCH && ($#load+1) < $UNROLL1) { - push (@load,$slot); - output ("p$#load = a[$#load]; q$#load = b[$#load];\n"); - $slot++; - } - # do next multiply - if ($#load > $#mul && $slot >= ($load[$#mul+1] + $LAT1) && - ($#mul+1) < $UNROLL1) { - push (@mul,$slot); - if ($LAT2 > 1) { - output ("m$#mul = p$#mul * q$#mul;\n"); - } - else { - output ("sum += p$#mul * q$#mul;\n"); - last if ($#mul+1) >= $UNROLL1; - } - $slot++; - } - # do next add - if ($LAT2 > 1) { - if ($#mul > $#add && $slot >= ($mul[$#add+1] + $LAT2)) { - push (@add,$slot); - output ("sum += m$#add;\n"); - $slot++; - last if ($#add+1) >= $UNROLL1; - } - } - - if ($slot == $startslot) { - # comment ("nop"); - $slot++; - } -} - -output ("a += $UNROLL1;\n"); -output ("b += $UNROLL1;\n"); -output ("n -= $UNROLL1;\n"); -output ("}\n"); - -output (<<END); -n += $UNROLL1; -while (n > 0) { -sum += (*a) * (*b); -a++; -b++; -n--; -} -return sum; -} -END diff --git a/extern/ode/dist/ode/fbuild/BuildLDLT b/extern/ode/dist/ode/fbuild/BuildLDLT deleted file mode 100644 index fd74faf18df..00000000000 --- a/extern/ode/dist/ode/fbuild/BuildLDLT +++ /dev/null @@ -1,654 +0,0 @@ -#!/usr/bin/perl -# -######################################################################### -# # -# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. # -# All rights reserved. Email: russ@q12.org Web: www.q12.org # -# # -# This library is free software; you can redistribute it and/or # -# modify it under the terms of EITHER: # -# (1) The GNU Lesser General Public License as published by the Free # -# Software Foundation; either version 2.1 of the License, or (at # -# your option) any later version. The text of the GNU Lesser # -# General Public License is included with this library in the # -# file LICENSE.TXT. # -# (2) The BSD-style license that is included with this library in # -# the file LICENSE-BSD.TXT. # -# # -# This library is distributed in the hope that it will be useful, # -# but WITHOUT ANY WARRANTY; without even the implied warranty of # -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files # -# LICENSE.TXT and LICENSE-BSD.TXT for more details. # -# # -######################################################################### - -# -# triangular matrix solver and factorizer code generator. -# -# SOLVER -# ------ -# -# if L is an n x n lower triangular matrix (with ones on the diagonal), the -# solver solves L*X=B where X and B are n x m matrices. this is the core -# step in L*D*L' factorization. the algorithm is (in matlab): -# -# for i=1:n -# for j=1:m -# X(i,j) = B(i,j) - L(i,1:i-1)*X(1:i-1,j); -# end -# end -# -# note that the ordering of the (i,j) loop is somewhat arbitrary. the only -# prerequisite to calculating element (i,j) of X is that all X(1:i-1,j) have -# have already been calcuated. this gives us some flexibility. -# -# the code generated below calculates X in N1 x N1 blocks. to speed up the -# innermost dot product loop, the outer product trick is used. for instance, -# to calculate the value of the 2x2 matrix ABCD below we first iterate over -# the vectors (a,b,c,d) and (e,f,g,h), computing ABCD = a*e+b*f+c*g+d*h. -# then A and B contain the dot product values needed in the algorithm, and -# C and D have almost all of it. the outer product trick reduces the number -# of memory loads required. in this example 16 loads are required, but if -# the simple dot product in the above algorithm is used then 32 loads are -# required. increasing N1 decreases the total number of loads, but only as long -# as we have enough temporary registers to keep the matrix blocks and vectors. -# -# L * X = B -# -# [ . ] [ e e ] [ . . ] -# [ . . ] [ f f ] [ . . ] -# [ . . . ] [ g g ] [ . . ] -# [ . . . . ] [ h h ] [ . . ] -# [ a b c d . ] [ A B ] = [ . . ] -# [ a b c d . . ] [ C D ] [ . . ] -# [ . . . . . . . ] [ . . ] [ . . ] -# [ . . . . . . . . ] [ . . ] [ . . ] -# [ . . . . . . . . . ] [ . . ] [ . . ] -# -# note that L is stored by rows but X and B are stored by columns. -# the outer product loops are unrolled for extra speed. -# -# LDLT FACTORIZATION -# ------------------ -# -# the factorization algorithm builds L incrementally by repeatedly solving -# the following equation: -# -# [ L 0 ] [ D 0 ] [ L' l ] = [ A a ] <-- n rows -# [ l' e ] [ 0 d ] [ 0 e' ] [ a' b ] <-- m rows -# -# [ L*D*L' L*D*l ] = [ A a ] -# [ l'*D*L' l'*D*l+e*d*e' ] [ a' b ] -# -# L*D*L'=A is an existing solution, and a,b are new rows/columns to add to A. -# we compute: -# -# L * (Dl) = a -# l = inv(D) * Dl -# e*d*e' = b - l'*Dl (m*m LDLT factorization) -# -# -# L-transpose solver -# ------------------ -# -# the LT (L-transpose) solver uses the same logic as the standard L-solver, -# with a few tricks to make it work. to solve L^T*X=B we first remap: -# L to Lhat : Lhat(i,j) = L(n-j,n-i) -# X to Xhat : Xhat(i) = X(n-i) -# B to Bhat : Bhat(i) = B(n-i) -# and then solve Lhat*Xhat = Bhat. the current LT solver only supports one -# right hand side, but that's okay as it is not used in the factorizer. -# -############################################################################# -# -# code generation parameters, set in a parameters file: -# FNAME : name of source file to generate - a .c file will be made -# TYPE : 'f' to build factorizer, 's' to build solver, 't' to build the -# transpose solver. -# N1 : block size (size of outer product matrix) (1..9) -# UNROLL1 : solver inner loop unrolling factor (1..) -# UNROLL2 : factorizer inner loop unrolling factor (1..) -# MADD : if nonzero, generate code for fused multiply-add (0,1) -# FETCH : how to fetch data in the inner loop: -# 0 - load in a batch (the `normal way') -# 1 - delay inner loop loads until just before they're needed -# -############################################################################# -# -# TODO -# ---- -# -# * dFactorLDLT() is not so efficient for matrix sizes < block size, e.g. -# redundant calls, zero loads, adds etc -# -############################################################################# -# -# NOTES: -# -# * on the pentium we can prefetch like this: -# asm ("prefetcht0 %0" : : "m" (*Ai) ); -# but it doesn't seem to help much - -require ("BuildUtil"); - -# get and check code generation parameters -error ("Usage: BuildLDLT <parameters-file>") if $#ARGV != 0; -do $ARGV[0]; - -if (!defined($FNAME) || !defined($TYPE) || !defined($N1) || - !defined($UNROLL1) || !defined($UNROLL2) || !defined($MADD) || - !defined($FETCH)) { - error ("code generation parameters not defined"); -} - -# check parameters -error ("bad TYPE") if $TYPE ne 'f' && $TYPE ne 's' && $TYPE ne 't'; -error ("bad N1") if $N1 < 1 || $N1 > 9; -error ("bad UNROLL1") if $UNROLL1 < 1; -error ("bad UNROLL2") if $UNROLL2 < 1; -error ("bad MADD") if $MADD != 0 && $MADD != 1; -error ("bad FETCH") if $FETCH < 0 && $FETCH > 1; - -############################################################################# -# utility - -# functions to handle delayed loading of p and q values. -# bit in the the `ploaded' and `qloaded' numbers record what has been loaded, -# so we dont load it again. - -sub newLoads -{ - # bits in these numbers say what registers p and q have been loaded so far - $ploaded = 0; - $qloaded = 0; -} - -sub loadedEverything -{ - $ploaded = 0xffffffff; - $qloaded = 0xffffffff; -} - -sub loadP # (i,loadcmd) -{ - my $i = $_[0]; - my $loadcmd = $_[1]; - return if ($ploaded & (1 << $i)); - output ($loadcmd); - $ploaded |= (1 << $i); -} - -sub loadQ # (i,loadcmd) -{ - my $i = $_[0]; - my $loadcmd = $_[1]; - return if ($qloaded & (1 << $i)); - output ($loadcmd); - $qloaded |= (1 << $i); -} - -############################################################################# -# make a fast L solve function. -# this function has a restriction that the leading dimension of X and B must -# be a multiple of the block size. - -sub innerOuterProductLoop # (M,k,nrhs,increment) -{ - my $M=$_[0]; - my $k=$_[1]; - my $nrhs=$_[2]; - my $increment=$_[3]; - my ($i,$j); - newLoads; - if ($FETCH==0) { - comment ("load p and q values"); - for ($i=1; $i<=$M; $i++) { - if ($TYPE eq 't') { - output ("p$i=ell[".ofs2(-($i-1),0,'lskip')."];\n"); - output ("q$i=ex[".ofs2(-($k),$i-1,'lskip')."];\n") if $i <= $nrhs; - } - else { - output ("p$i=ell[".ofs2($k,$i-1,'lskip')."];\n"); - output ("q$i=ex[".ofs2($k,$i-1,'lskip')."];\n") if $i <= $nrhs; - } - } - loadedEverything; - } - - comment ("compute outer product and add it to the Z matrix"); - for ($i=1; $i<=$M; $i++) { - for ($j=1; $j<=$nrhs; $j++) { - if ($TYPE eq 't') { - loadP ($i,"p$i=ell[".ofs2(-($i-1),0,'lskip')."];\n"); - loadQ ($j,"q$j=ex[".ofs2(-($k),$j-1,'lskip')."];\n"); - } - else { - loadP ($i,"p$i=ell[".ofs2($k,$i-1,'lskip')."];\n"); - loadQ ($j,"q$j=ex[".ofs2($k,$j-1,'lskip')."];\n"); - } - my $var = $MADD ? "Z$i$j +=" : "m$i$j ="; - output ("$var p$i * q$j;\n"); - } - } - - if ($TYPE eq 't') { - if ($increment > 0) { - output ("ell += lskip1;\n"); - output ("ex -= $increment;\n"); - } - else { - output ("ell += lskip1;\n"); - } - } - else { - if ($increment > 0) { - comment ("advance pointers"); - output ("ell += $increment;\n"); - output ("ex += $increment;\n"); - } - } - - if ($MADD==0) { - for ($i=1; $i<=$M; $i++) { - for ($j=1; $j<=$nrhs; $j++) { - output ("Z$i$j += m$i$j;\n"); - } - } - } -} - - -sub computeRows # (nrhs,rows) -{ - my $nrhs = $_[0]; - my $rows = $_[1]; - my ($i,$j,$k); - - comment ("compute all $rows x $nrhs block of X, from rows i..i+$rows-1"); - - comment ("set the Z matrix to 0"); - for ($i=1; $i<=$rows; $i++) { - for ($j=1; $j<=$nrhs; $j++) { - output ("Z$i$j=0;\n"); - } - } - if ($TYPE eq 't') { - output ("ell = L - i;\n"); - } - else { - output ("ell = L + i*lskip1;\n"); - } - output ("ex = B;\n"); - - comment ("the inner loop that computes outer products and adds them to Z"); - output ("for (j=i-$UNROLL1; j >= 0; j -= $UNROLL1) {\n"); - for ($k=0; $k < $UNROLL1; $k++) { - innerOuterProductLoop ($rows,$k,$nrhs,($k==$UNROLL1-1) ? $UNROLL1 : 0); - } - - comment ("end of inner loop"); - output ("}\n"); - - if ($UNROLL1 > 1) { - comment ("compute left-over iterations"); - output ("j += $UNROLL1;\n"); - output ("for (; j > 0; j--) {\n"); - innerOuterProductLoop ($rows,'0',$nrhs,1); - output ("}\n"); - } - - comment ("finish computing the X(i) block"); - - for ($j=1; $j<=$nrhs; $j++) { - if ($TYPE eq 't') { - output ("Z1$j = ex[".ofs1(-($j-1),'lskip')."] - Z1$j;\n"); - output ("ex[".ofs1(-($j-1),'lskip')."] = Z1$j;\n"); - } - else { - output ("Z1$j = ex[".ofs1($j-1,'lskip')."] - Z1$j;\n"); - output ("ex[".ofs1($j-1,'lskip')."] = Z1$j;\n"); - } - } - - for ($i=2; $i<=$rows; $i++) { - for ($j=1; $j<$i; $j++) { - if ($TYPE eq 't') { - output ("p$j = ell[".ofs2(-($i-1),$j-1,'lskip')."];\n"); - } - else { - output ("p$j = ell[".ofs2($j-1,$i-1,'lskip')."];\n"); - } - } - for ($j=1; $j<=$nrhs; $j++) { - if ($TYPE eq 't') { - output ("Z$i$j = ex[".ofs2(-($i-1),$j-1,'lskip')."] - Z$i$j"); - } - else { - output ("Z$i$j = ex[".ofs2($i-1,$j-1,'lskip')."] - Z$i$j"); - } - for ($k=1; $k < $i; $k++) { - output (" - p$k*Z$k$j"); - } - output (";\n"); - if ($TYPE eq 't') { - output ("ex[".ofs2(-($i-1),$j-1,'lskip')."] = Z$i$j;\n"); - } - else { - output ("ex[".ofs2($i-1,$j-1,'lskip')."] = Z$i$j;\n"); - } - } - } -} - - -sub makeFastL1Solve # ( number-of-right-hand-sides ) -{ - my $nrhs = $_[0]; - my ($i,$j,$k); - my $funcsuffix = ($TYPE eq 'f') ? "_$nrhs" : ''; - my $staticvoid = ($TYPE eq 'f') ? 'static void' : 'void'; - - # function header - if ($TYPE eq 't') { - output (<<END); - -/* solve L^T * x=b, with b containing 1 right hand side. - * L is an n*n lower triangular matrix with ones on the diagonal. - * L is stored by rows and its leading dimension is lskip. - * b is an n*1 matrix that contains the right hand side. - * b is overwritten with x. - * this processes blocks of $N1. - */ - -void dSolveL1T (const dReal *L, dReal *B, int n, int lskip1) -{ -END - } - else { - output (<<END); - -/* solve L*X=B, with B containing $nrhs right hand sides. - * L is an n*n lower triangular matrix with ones on the diagonal. - * L is stored by rows and its leading dimension is lskip. - * B is an n*$nrhs matrix that contains the right hand sides. - * B is stored by columns and its leading dimension is also lskip. - * B is overwritten with X. - * this processes blocks of $N1*$N1. - * if this is in the factorizer source file, n must be a multiple of $N1. - */ - -$staticvoid dSolveL1$funcsuffix (const dReal *L, dReal *B, int n, int lskip1) -{ -END - } - - comment ("declare variables - Z matrix, p and q vectors, etc"); - output ("dReal "); - for ($i=1; $i<=$N1; $i++) { - for ($j=1; $j<=$nrhs; $j++) { - output ("Z$i$j,"); # Z matrix - output ("m$i$j,") if ! $MADD; # temporary vars where multiplies put - } - } - for ($i=1; $i<=$N1; $i++) { - output ("p$i,"); - output ("q$i,") if $i <= $nrhs; - } - output ("*ex;\nconst dReal *ell;\n"); - output ("int "); - for ($i=2; $i<$N1; $i++) { - output ("lskip$i,"); - } - output ("i,j;\n"); - - if ($TYPE eq 't') { - comment ("special handling for L and B because we're solving L1 *transpose*"); - output ("L = L + (n-1)*(lskip1+1);\n"); - output ("B = B + n-1;\n"); - output ("lskip1 = -lskip1;\n"); - } - - if ($N1 > 2) { - comment ("compute lskip values"); - for ($i=2; $i<$N1; $i++) { - output ("lskip$i = $i*lskip1;\n"); - } - } - - comment ("compute all $N1 x $nrhs blocks of X"); - if ($TYPE eq 's' or $TYPE eq 't') { - output ("for (i=0; i <= n-$N1; i+=$N1) {\n"); - } - else { - output ("for (i=0; i < n; i+=$N1) {\n"); - } - computeRows ($nrhs,$N1); - comment ("end of outer loop"); - output ("}\n"); - - if ($TYPE eq 's' or $TYPE eq 't') { - comment ("compute rows at end that are not a multiple of block size"); - output ("for (; i < n; i++) {\n"); - computeRows ($nrhs,1); - output ("}\n"); - } - - output ("}\n"); -} - -############################################################################# -# make a fast L*D*L' factorizer - -# code fragment: this factors an M x M block. if A_or_Z is 0 then it works -# on the $A matrix otherwise it works on the Z matrix. in either case it -# writes the diagonal entries into the `dee' vector. -# it is a simple implementation of the LDLT algorithm, with no tricks. - -sub getA # (i,j,A,A_or_Z) -{ - my $i = $_[0]; - my $j = $_[1]; - my $A = $_[2]; - return $_[3] ? ('Z'.($i+1).($j+1)) : ($A.'['.ofs2($j,$i,'nskip').']'); -} - -sub miniLDLT # (A,A_or_Z,M) -{ - my ($i,$j,$k); - my $A = $_[0]; - my $AZ = $_[1]; - my $M = $_[2]; - comment ("factorize $M x $M block " . ($AZ ? "Z,dee" : "$A,dee")); - comment ("factorize row 1"); - output ("dee[0] = dRecip(".getA(0,0,$A,$AZ).");\n"); - for ($i=1; $i<$M; $i++) { - comment ("factorize row ".($i+1)); - for ($j=1; $j<$i; $j++) { - output (getA($i,$j,$A,$AZ)." -= "); - for ($k=0; $k<$j; $k++) { - output (" + ") if $k > 0; - output (getA($i,$k,$A,$AZ)."*".getA($j,$k,$A,$AZ)); - } - output (";\n"); - } - output ("sum = 0;\n"); - for ($j=0; $j<$i; $j++) { - output ("q1 = ".getA($i,$j,$A,$AZ).";\n"); - output ("q2 = q1 * dee[$j];\n"); - output (getA($i,$j,$A,$AZ)." = q2;\n"); - output ("sum += q1*q2;\n"); - } - output ("dee[$i] = dRecip(".getA($i,$i,$A,$AZ)." - sum);\n"); - } - comment ("done factorizing $M x $M block"); -} - - -sub innerScaleAndOuterProductLoop # (M,k) -{ - my $M = $_[0]; - my $k = $_[1]; - my ($i,$j); - for ($i=1; $i<=$M; $i++) { - output ("p$i = ell[".ofs2($k,$i-1,'nskip')."];\n"); - } - output ("dd = dee[$k];\n"); - for ($i=1; $i<=$M; $i++) { - output ("q$i = p$i*dd;\n"); - } - for ($i=1; $i<=$M; $i++) { - output ("ell[".ofs2($k,$i-1,'nskip')."] = q$i;\n"); - } - for ($i=1; $i<=$M; $i++) { - for ($j=1; $j<=$i; $j++) { - my $var = $MADD ? "Z$i$j +=" : "m$i$j ="; - output ("$var p$i*q$j;\n"); - } - } - if ($MADD==0) { - for ($i=1; $i<=$M; $i++) { - for ($j=1; $j<=$i; $j++) { - output ("Z$i$j += m$i$j;\n"); - } - } - } -} - - -sub diagRows # (M) -{ - my $M=$_[0]; - comment ("scale the elements in a $M x i block at A(i,0), and also"); - comment ("compute Z = the outer product matrix that we'll need."); - for ($i=1; $i<=$M; $i++) { - for ($j=1; $j<=$i; $j++) { - output ("Z$i$j = 0;\n"); - } - } - output ("ell = A+i*nskip1;\n"); - output ("dee = d;\n"); - output ("for (j=i-$UNROLL2; j >= 0; j -= $UNROLL2) {\n"); - for ($i=0; $i < $UNROLL2; $i++) { - innerScaleAndOuterProductLoop ($M,$i); - } - output ("ell += $UNROLL2;\n"); - output ("dee += $UNROLL2;\n"); - output ("}\n"); - - if ($UNROLL2 > 1) { - comment ("compute left-over iterations"); - output ("j += $UNROLL2;\n"); - output ("for (; j > 0; j--) {\n"); - innerScaleAndOuterProductLoop ($M,0); - output ("ell++;\n"); - output ("dee++;\n"); - output ("}\n"); - } -} - - -sub diagBlock # (M) -{ - my $M = $_[0]; - comment ("solve for diagonal $M x $M block at A(i,i)"); - for ($i=1; $i<=$M; $i++) { - for ($j=1; $j<=$i; $j++) { - output ("Z$i$j = ell[".ofs2($j-1,$i-1,'nskip')."] - Z$i$j;\n"); - } - } - output ("dee = d + i;\n"); - miniLDLT ('',1,$M); - for ($i=2; $i<=$M; $i++) { - for ($j=1; $j<$i; $j++) { - output ("ell[".ofs2($j-1,$i-1,'nskip')."] = Z$i$j;\n"); - } - } -} - - -sub makeFastLDLT -{ - my ($i,$j,$k); - - # function header - output (<<END); - - -void dFactorLDLT (dReal *A, dReal *d, int n, int nskip1) -{ -END - output ("int i,j"); - for ($i=2; $i<$N1; $i++) { - output (",nskip$i"); - } - output (";\n"); - output ("dReal sum,*ell,*dee,dd,p1,p2"); - for ($i=3; $i<=$N1; $i++) { - output (",p$i"); - } - for ($i=1; $i<=$N1; $i++) { - output (",q$i"); - } - for ($i=1; $i<=$N1; $i++) { - for ($j=1; $j<=$i; $j++) { - output (",Z$i$j"); - output (",m$i$j") if ! $MADD; # temporary vars where multiplies put - } - } - output (";\n"); - output ("if (n < 1) return;\n"); - # output ("nskip1 = dPAD(n);\n"); ... not any more - for ($i=2; $i<$N1; $i++) { - output ("nskip$i = $i*nskip1;\n"); - } - - output ("\nfor (i=0; i<=n-$N1; i += $N1) {\n"); - comment ("solve L*(D*l)=a, l is scaled elements in $N1 x i block at A(i,0)"); - output ("dSolveL1_$N1 (A,A+i*nskip1,i,nskip1);\n"); - - diagRows ($N1); - diagBlock ($N1); - output ("}\n"); - - comment ("compute the (less than $N1) rows at the bottom"); - output ("switch (n-i) {\n"); - output ("case 0:\n"); - output ("break;\n\n"); - - for ($i=1; $i<$N1; $i++) { - output ("case $i:\n"); - output ("dSolveL1_$i (A,A+i*nskip1,i,nskip1);\n"); - diagRows ($i); - diagBlock ($i); - output ("break;\n\n"); - } - - output ("default: *((char*)0)=0; /* this should never happen! */\n"); - output ("}\n"); - - output ("}\n"); -} - -############################################################################# -# write source code - -open (FOUT,">$FNAME.c") or die "can't open $FNAME.c for writing"; - -# file and function header -output (<<END); -/* generated code, do not edit. */ - -#include "ode/matrix.h" -END - -if ($TYPE eq 'f') { - for ($i=1; $i <= $N1; $i++) { - makeFastL1Solve ($i); - } - makeFastLDLT; -} -else { - makeFastL1Solve (1); - makeRealFastL1Solve; -} -close FOUT; diff --git a/extern/ode/dist/ode/fbuild/BuildMultidot b/extern/ode/dist/ode/fbuild/BuildMultidot deleted file mode 100644 index f6b117708c5..00000000000 --- a/extern/ode/dist/ode/fbuild/BuildMultidot +++ /dev/null @@ -1,174 +0,0 @@ -#!/usr/bin/perl -# -######################################################################### -# # -# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. # -# All rights reserved. Email: russ@q12.org Web: www.q12.org # -# # -# This library is free software; you can redistribute it and/or # -# modify it under the terms of EITHER: # -# (1) The GNU Lesser General Public License as published by the Free # -# Software Foundation; either version 2.1 of the License, or (at # -# your option) any later version. The text of the GNU Lesser # -# General Public License is included with this library in the # -# file LICENSE.TXT. # -# (2) The BSD-style license that is included with this library in # -# the file LICENSE-BSD.TXT. # -# # -# This library is distributed in the hope that it will be useful, # -# but WITHOUT ANY WARRANTY; without even the implied warranty of # -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files # -# LICENSE.TXT and LICENSE-BSD.TXT for more details. # -# # -######################################################################### - -# multi-dot-product code generator. this code generator is based on the -# dot-product code generator. -# -# code generation parameters, set in a parameters file: -# FNAME : name of source file to generate - a .c file will be made -# N1 : block size (number of `a' vectors to dot) -# UNROLL1 : inner loop unrolling factor (1..) -# FETCH : max num of a[i]'s and b[i]'s to load ahead of muls -# LAT1 : load -> mul latency (>=1) -# LAT2 : mul -> add latency (>=1). if this is 1, use fused mul-add -# -############################################################################# - -require ("BuildUtil"); - -# get and check code generation parameters -error ("Usage: BuildMultidot <parameters-file>") if $#ARGV != 0; -do $ARGV[0]; - -if (!defined($FNAME) || !defined($N1) || !defined($UNROLL1) || - !defined($FETCH) || !defined($LAT1) || !defined($LAT2)) { - error ("code generation parameters not defined"); -} - -# check parameters -error ("bad N1") if $N1 < 2; -error ("bad UNROLL1") if $UNROLL1 < 1; -error ("bad FETCH") if $FETCH < 1; -error ("bad LAT1") if $LAT1 < 1; -error ("bad LAT2") if $LAT2 < 1; - -############################################################################# - -open (FOUT,">$FNAME.c") or die "can't open $FNAME.c for writing"; - -# file and function header -output (<<END); -/* generated code, do not edit. */ - -#include "ode/matrix.h" - - -END -output ("void dMultidot$N1 ("); -for ($i=0; $i<$N1; $i++) { - output ("const dReal *a$i, "); -} -output ("const dReal *b, dReal *outsum, int n)\n{\n"); - -output ("dReal "); -for ($i=0; $i<$UNROLL1; $i++) { - for ($j=0; $j<$N1; $j++) { - output ("p$i$j,"); - output ("m$i$j,") if $LAT2 > 1; - } - output ("q$i,"); -} -for ($i=0; $i<$N1; $i++) { - output ("sum$i"); - output (",") if $i < ($N1-1); -} -output (";\n"); -for ($i=0; $i<$N1; $i++) { - output ("sum$i = 0;\n"); -} -output (<<END); -n -= $UNROLL1; -while (n >= 0) { -END - -@load = (); # slot where a[i]'s and b[i]'s loaded -@mul = (); # slot where multiply i happened -@add = (); # slow where add i happened - -for ($i=0; $i<$N1; $i++) { - output ("p0$i = a$i [0];\n"); -} -output ("q0 = b[0];\n"); -push (@load,0); - -$slot=0; # one slot for every load/mul/add/nop issued -for (;;) { - $startslot = $slot; - - # do next load - if (($#load - $#mul) < $FETCH && ($#load+1) < $UNROLL1) { - push (@load,$slot); - for ($j=0; $j<$N1; $j++) { - output ("p$#load$j = a$j [$#load];\n"); - } - output ("q$#load = b[$#load];\n"); - $slot++; - } - - # do next multiply - if ($#load > $#mul && $slot >= ($load[$#mul+1] + $LAT1) && - ($#mul+1) < $UNROLL1) { - push (@mul,$slot); - if ($LAT2 > 1) { - for ($j=0; $j<$N1; $j++) { - output ("m$#mul$j = p$#mul$j * q$#mul;\n"); - } - } - else { - for ($j=0; $j<$N1; $j++) { - output ("sum$j += p$#mul$j * q$#mul;\n"); - } - last if ($#mul+1) >= $UNROLL1; - } - $slot++; - } - # do next add - if ($LAT2 > 1) { - if ($#mul > $#add && $slot >= ($mul[$#add+1] + $LAT2)) { - push (@add,$slot); - for ($j=0; $j<$N1; $j++) { - output ("sum$j += m$#add$j;\n"); - } - $slot++; - last if ($#add+1) >= $UNROLL1; - } - } - - if ($slot == $startslot) { - # comment ("nop"); - $slot++; - } -} - -for ($j=0; $j<$N1; $j++) { - output ("a$j += $UNROLL1;\n"); -} -output ("b += $UNROLL1;\n"); -output ("n -= $UNROLL1;\n"); -output ("}\n"); - -output ("n += $UNROLL1;\n"); -output ("while (n > 0) {\n"); -output ("q0 = *b;\n"); -for ($j=0; $j<$N1; $j++) { - output ("sum$j += (*a$j) * q0;\n"); - output ("a$j++;\n"); -} -output ("b++;\n"); -output ("n--;\n"); -output ("}\n"); -for ($j=0; $j<$N1; $j++) { - output ("outsum[$j] = sum$j;\n"); -} -output ("}\n"); diff --git a/extern/ode/dist/ode/fbuild/BuildUtil b/extern/ode/dist/ode/fbuild/BuildUtil deleted file mode 100644 index b0828ff5ec1..00000000000 --- a/extern/ode/dist/ode/fbuild/BuildUtil +++ /dev/null @@ -1,99 +0,0 @@ -#!/usr/bin/perl -w -# -######################################################################### -# # -# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. # -# All rights reserved. Email: russ@q12.org Web: www.q12.org # -# # -# This library is free software; you can redistribute it and/or # -# modify it under the terms of EITHER: # -# (1) The GNU Lesser General Public License as published by the Free # -# Software Foundation; either version 2.1 of the License, or (at # -# your option) any later version. The text of the GNU Lesser # -# General Public License is included with this library in the # -# file LICENSE.TXT. # -# (2) The BSD-style license that is included with this library in # -# the file LICENSE-BSD.TXT. # -# # -# This library is distributed in the hope that it will be useful, # -# but WITHOUT ANY WARRANTY; without even the implied warranty of # -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files # -# LICENSE.TXT and LICENSE-BSD.TXT for more details. # -# # -######################################################################### - -package BuildUtil; - - -# print out code. after newlines, indent according to the number of curly -# brackets we've seen - -my $indent = 0; -my $startofline = 1; - - -sub main::output -{ - my $line = $_[0]; - my ($i,$j,$c); - for ($i=0; $i < length ($line); $i++) { - $c = substr ($line,$i,1); - print main::FOUT $c if $c eq '{'; - $indent++ if $c eq '{'; - $indent-- if $c eq '}'; - if ($startofline) { - for ($j=0; $j < $indent; $j++) { - print main::FOUT " "; - } - $startofline = 0; - } - print main::FOUT $c if $c ne '{'; - $startofline = 1 if $c eq "\n"; - } -} - - -# write a C comment with the correct indenting - -sub main::comment -{ - main::output ("/* $_[0] */\n"); -} - - -# return an offset: N*skip = skipN where N=0,1,2,... - -sub main::ofs1 # (N,skip) -{ - my $N = $_[0]; - my $skip = $_[1]; - return '0' if $N==0; - return $skip . $N; -} - - -# return an offset: M+N*skip = M+skipN where N=0,1,2,... - -sub main::ofs2 # (M,N,skip) -{ - my $M = $_[0]; - my $N = $_[1]; - my $skip = $_[2]; - $M = '0' if $M eq '-0'; - my $a = $M; - $a .= '+' . $skip . $N if ($N > 0); - substr ($a,0,2)='' if substr ($a,0,2) eq '0+'; - return $a; -} - - -# print an error message and exit - -sub main::error -{ - print "ERROR: $_[0]\n"; - exit 1; -} - - -1; diff --git a/extern/ode/dist/ode/fbuild/Dependencies b/extern/ode/dist/ode/fbuild/Dependencies deleted file mode 100644 index 86f5cd4417b..00000000000 --- a/extern/ode/dist/ode/fbuild/Dependencies +++ /dev/null @@ -1,16 +0,0 @@ -test_dot.o: test_dot.cpp ../../include/ode/ode.h \ - ../../include/ode/config.h ../../include/ode/contact.h \ - ../../include/ode/common.h ../../include/ode/error.h \ - ../../include/ode/memory.h ../../include/ode/odemath.h \ - ../../include/ode/matrix.h ../../include/ode/timer.h \ - ../../include/ode/rotation.h ../../include/ode/mass.h \ - ../../include/ode/space.h ../../include/ode/geom.h \ - ../../include/ode/misc.h -test_ldlt.o: test_ldlt.cpp ../../include/ode/ode.h \ - ../../include/ode/config.h ../../include/ode/contact.h \ - ../../include/ode/common.h ../../include/ode/error.h \ - ../../include/ode/memory.h ../../include/ode/odemath.h \ - ../../include/ode/matrix.h ../../include/ode/timer.h \ - ../../include/ode/rotation.h ../../include/ode/mass.h \ - ../../include/ode/space.h ../../include/ode/geom.h \ - ../../include/ode/misc.h diff --git a/extern/ode/dist/ode/fbuild/Makefile b/extern/ode/dist/ode/fbuild/Makefile deleted file mode 100644 index f988c3739af..00000000000 --- a/extern/ode/dist/ode/fbuild/Makefile +++ /dev/null @@ -1,77 +0,0 @@ -######################################################################### -# # -# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. # -# All rights reserved. Email: russ@q12.org Web: www.q12.org # -# # -# This library is free software; you can redistribute it and/or # -# modify it under the terms of EITHER: # -# (1) The GNU Lesser General Public License as published by the Free # -# Software Foundation; either version 2.1 of the License, or (at # -# your option) any later version. The text of the GNU Lesser # -# General Public License is included with this library in the # -# file LICENSE.TXT. # -# (2) The BSD-style license that is included with this library in # -# the file LICENSE-BSD.TXT. # -# # -# This library is distributed in the hope that it will be useful, # -# but WITHOUT ANY WARRANTY; without even the implied warranty of # -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files # -# LICENSE.TXT and LICENSE-BSD.TXT for more details. # -# # -######################################################################### - -# currently this only works under linux, and it's a bit of a mess! - -MAKEFILE_INC=../../build/Makefile.inc -include $(MAKEFILE_INC) - -INCLUDE_PATHS=../../include -LIB_PATHS = ../../lib -DEFINES=dDOUBLE - -SOURCES_CPP=test_ldlt.cpp -SOURCES_C=fastldlt.c fastlsolve.c fastltsolve.c -APPS=$(call fEXENAME,test_ldlt) $(call fEXENAME,test_dot) $(call fEXENAME,test_multidot) -EXTRA_CLEAN=test_ldlt test_dot test_multidot fastldlt.c fastlsolve.c fastltsolve.c fastdot.c fastmultidot.c - - -all: $(APPS) - -$(call fEXENAME,test_ldlt): $(call fTARGETS,$(SOURCES_CPP) $(SOURCES_C)) - gcc -o $@ $^ -L $(LIB_PATHS) $(call fLIB,ode) -lm - -$(call fEXENAME,test_dot): test_dot.o fastdot.o - gcc -o $@ test_dot.o fastdot.o -L $(LIB_PATHS) $(call fLIB,ode) -lm - -$(call fEXENAME,test_multidot): test_multidot.o fastmultidot.o - gcc -o $@ test_multidot.o fastmultidot.o -L $(LIB_PATHS) $(call fLIB,ode) -lm - -fastldlt.o: fastldlt.c - gcc -O1 -I$(INCLUDE_PATHS) -ffast-math -fomit-frame-pointer -c -D$(DEFINES) $< - -fastlsolve.o: fastlsolve.c - gcc -O1 -I$(INCLUDE_PATHS) -ffast-math -fomit-frame-pointer -c -D$(DEFINES) $< - -fastltsolve.o: fastltsolve.c - gcc -O1 -I$(INCLUDE_PATHS) -ffast-math -fomit-frame-pointer -c -D$(DEFINES) $< - -fastdot.o: fastdot.c - gcc -O1 -I$(INCLUDE_PATHS) -ffast-math -fomit-frame-pointer -c -D$(DEFINES) $< - -fastmultidot.o: fastmultidot.c - gcc -O1 -I$(INCLUDE_PATHS) -ffast-math -fomit-frame-pointer -c -D$(DEFINES) $< - -fastldlt.c: BuildLDLT BuildUtil ParametersF - ./BuildLDLT ParametersF - -fastlsolve.c: BuildLDLT BuildUtil ParametersS - ./BuildLDLT ParametersS - -fastltsolve.c: BuildLDLT BuildUtil ParametersT - ./BuildLDLT ParametersT - -fastdot.c: BuildDot BuildUtil ParametersD - ./BuildDot ParametersD - -fastmultidot.c: BuildMultidot BuildUtil ParametersM - ./BuildMultidot ParametersM diff --git a/extern/ode/dist/ode/fbuild/OptimizeDot b/extern/ode/dist/ode/fbuild/OptimizeDot deleted file mode 100644 index 1dc178262aa..00000000000 --- a/extern/ode/dist/ode/fbuild/OptimizeDot +++ /dev/null @@ -1,71 +0,0 @@ -#!/usr/bin/perl -# -######################################################################### -# # -# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. # -# All rights reserved. Email: russ@q12.org Web: www.q12.org # -# # -# This library is free software; you can redistribute it and/or # -# modify it under the terms of EITHER: # -# (1) The GNU Lesser General Public License as published by the Free # -# Software Foundation; either version 2.1 of the License, or (at # -# your option) any later version. The text of the GNU Lesser # -# General Public License is included with this library in the # -# file LICENSE.TXT. # -# (2) The BSD-style license that is included with this library in # -# the file LICENSE-BSD.TXT. # -# # -# This library is distributed in the hope that it will be useful, # -# but WITHOUT ANY WARRANTY; without even the implied warranty of # -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files # -# LICENSE.TXT and LICENSE-BSD.TXT for more details. # -# # -######################################################################### - -# optimize the dot product built by BuildDot - -############################################################################## - -require ("OptimizeUtil"); - -# unused standard parameters -$TYPE='unused'; -$N1=0; # unused -$UNROLL2=0; # unused -$MADD=0; # unused - -############################################################################## - -sub testDot # (filename) -{ - my $filename = $_[0]; - createParametersFile ('ParametersD'); - $params = "$N1 $UNROLL1 $UNROLL2 $MADD $FETCH $LAT1 $LAT2"; - print "***** TESTING $params\n"; - doit ("rm -f fastdot.c fastdot.o test_dot"); - doit ("make test_dot"); - doit ("./test_dot >> $filename"); - open (FILE,">>$filename"); - print FILE " $params\n"; - close FILE; -} - -# find optimal parameters. write results to data4.txt - -open (FILE,">data4.txt"); -print FILE "# dot product data from OptimizeDot\n"; -close FILE; -$FNAME='fastdot'; - -for ($UNROLL1=1; $UNROLL1 <= 10; $UNROLL1++) { - for ($LAT1=1; $LAT1 <= 5; $LAT1++) { - for ($LAT2=1; $LAT2 <= 5; $LAT2++) { - for ($FETCH=1; $FETCH<=5; $FETCH++) { - testDot ('data4.txt'); - } - } - } -} - -readBackDataFile ('data4.txt'); -createParametersFile ('ParametersD'); diff --git a/extern/ode/dist/ode/fbuild/OptimizeLDLT b/extern/ode/dist/ode/fbuild/OptimizeLDLT deleted file mode 100644 index 612633e00c2..00000000000 --- a/extern/ode/dist/ode/fbuild/OptimizeLDLT +++ /dev/null @@ -1,91 +0,0 @@ -#!/usr/bin/perl -# -######################################################################### -# # -# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. # -# All rights reserved. Email: russ@q12.org Web: www.q12.org # -# # -# This library is free software; you can redistribute it and/or # -# modify it under the terms of EITHER: # -# (1) The GNU Lesser General Public License as published by the Free # -# Software Foundation; either version 2.1 of the License, or (at # -# your option) any later version. The text of the GNU Lesser # -# General Public License is included with this library in the # -# file LICENSE.TXT. # -# (2) The BSD-style license that is included with this library in # -# the file LICENSE-BSD.TXT. # -# # -# This library is distributed in the hope that it will be useful, # -# but WITHOUT ANY WARRANTY; without even the implied warranty of # -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files # -# LICENSE.TXT and LICENSE-BSD.TXT for more details. # -# # -######################################################################### - -# optimize the factorizer built by BuildLDLT -# -# FNAME : name of source file to generate - .h and .c files will be made -# N1 : block size (size of outer product matrix) (1..9) -# UNROLL1 : solver inner loop unrolling factor (1..) -# UNROLL2 : factorizer inner loop unrolling factor (1..) -# MADD : if nonzero, generate code for fused multiply-add (0,1) -# FETCH : how to fetch data in the inner loop: -# 0 - load in a batch (the `normal way') -# 1 - delay inner loop loads until just before they're needed - -############################################################################## - -require ("OptimizeUtil"); - -############################################################################## -# optimize factorizer - -sub testFactorizer # (filename) -{ - my $filename = $_[0]; - createParametersFile ('ParametersF'); - $params = "$N1 $UNROLL1 $UNROLL2 $MADD $FETCH"; - print "***** TESTING $params\n"; - doit ("rm -f fastldlt.c fastldlt.o test_ldlt"); - doit ("make test_ldlt"); - doit ("./test_ldlt f >> $filename"); - open (FILE,">>$filename"); - print FILE " $params\n"; - close FILE; -} - - -# first find optimal parameters ignoring UNROLL1 and UNROLL2, write results -# to data1.txt - -open (FILE,">data1.txt"); -print FILE "# factorizer data from OptimizeLDLT\n"; -close FILE; -$FNAME='fastldlt'; -$TYPE='f'; -$UNROLL1=4; -$UNROLL2=4; -for ($N1=1; $N1 <= 4; $N1++) { - for ($MADD=0; $MADD<=1; $MADD++) { - for ($FETCH=0; $FETCH<=1; $FETCH++) { - testFactorizer ('data1.txt'); - } - } -} - -readBackDataFile ('data1.txt'); -createParametersFile ('ParametersF'); - -# now find optimal UNROLL1 and UNROLL2 values, write results to data2.txt - -open (FILE,">data2.txt"); -print FILE "# factorizer data from OptimizeLDLT\n"; -close FILE; -for ($UNROLL1=1; $UNROLL1 <= 10; $UNROLL1++) { - for ($UNROLL2=1; $UNROLL2 <= 10; $UNROLL2++) { - testFactorizer ('data2.txt'); - } -} - -readBackDataFile ('data2.txt'); -createParametersFile ('ParametersF'); diff --git a/extern/ode/dist/ode/fbuild/OptimizeLSolve b/extern/ode/dist/ode/fbuild/OptimizeLSolve deleted file mode 100644 index bba6b495f2c..00000000000 --- a/extern/ode/dist/ode/fbuild/OptimizeLSolve +++ /dev/null @@ -1,76 +0,0 @@ -#!/usr/bin/perl -# -######################################################################### -# # -# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. # -# All rights reserved. Email: russ@q12.org Web: www.q12.org # -# # -# This library is free software; you can redistribute it and/or # -# modify it under the terms of EITHER: # -# (1) The GNU Lesser General Public License as published by the Free # -# Software Foundation; either version 2.1 of the License, or (at # -# your option) any later version. The text of the GNU Lesser # -# General Public License is included with this library in the # -# file LICENSE.TXT. # -# (2) The BSD-style license that is included with this library in # -# the file LICENSE-BSD.TXT. # -# # -# This library is distributed in the hope that it will be useful, # -# but WITHOUT ANY WARRANTY; without even the implied warranty of # -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files # -# LICENSE.TXT and LICENSE-BSD.TXT for more details. # -# # -######################################################################### - -# optimize the solver built by BuildLDLT -# -# FNAME : name of source file to generate - .h and .c files will be made -# N1 : block size (size of outer product matrix) (1..9) -# UNROLL1 : solver inner loop unrolling factor (1..) -# UNROLL2 : factorizer inner loop unrolling factor (1..) -# MADD : if nonzero, generate code for fused multiply-add (0,1) -# FETCH : how to fetch data in the inner loop: -# 0 - load in a batch (the `normal way') -# 1 - delay inner loop loads until just before they're needed - -############################################################################## - -require ("OptimizeUtil"); - -############################################################################## -# optimize solver - -sub testSolver # (filename) -{ - my $filename = $_[0]; - createParametersFile ('ParametersS'); - $params = "$N1 $UNROLL1 $UNROLL2 $MADD $FETCH"; - print "***** TESTING $params\n"; - doit ("rm -f fastlsolve.c fastlsolve.o test_ldlt"); - doit ("make test_ldlt"); - doit ("./test_ldlt s >> $filename"); - open (FILE,">>$filename"); - print FILE " $params\n"; - close FILE; -} - -# find optimal parameters. UNROLL2 has no effect. write results to data3.txt - -open (FILE,">data3.txt"); -print FILE "# solver data from OptimizeLDLT\n"; -close FILE; -$FNAME='fastlsolve'; -$TYPE='s'; -$UNROLL2=1; -for ($N1=1; $N1 <= 5; $N1++) { - for ($UNROLL1=1; $UNROLL1 <= 15; $UNROLL1++) { - for ($MADD=0; $MADD<=1; $MADD++) { - for ($FETCH=0; $FETCH<=1; $FETCH++) { - testSolver ('data3.txt'); - } - } - } -} - -readBackDataFile ('data3.txt'); -createParametersFile ('ParametersS'); diff --git a/extern/ode/dist/ode/fbuild/OptimizeLTSolve b/extern/ode/dist/ode/fbuild/OptimizeLTSolve deleted file mode 100644 index a10109e2d2c..00000000000 --- a/extern/ode/dist/ode/fbuild/OptimizeLTSolve +++ /dev/null @@ -1,76 +0,0 @@ -#!/usr/bin/perl -# -######################################################################### -# # -# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. # -# All rights reserved. Email: russ@q12.org Web: www.q12.org # -# # -# This library is free software; you can redistribute it and/or # -# modify it under the terms of EITHER: # -# (1) The GNU Lesser General Public License as published by the Free # -# Software Foundation; either version 2.1 of the License, or (at # -# your option) any later version. The text of the GNU Lesser # -# General Public License is included with this library in the # -# file LICENSE.TXT. # -# (2) The BSD-style license that is included with this library in # -# the file LICENSE-BSD.TXT. # -# # -# This library is distributed in the hope that it will be useful, # -# but WITHOUT ANY WARRANTY; without even the implied warranty of # -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files # -# LICENSE.TXT and LICENSE-BSD.TXT for more details. # -# # -######################################################################### - -# optimize the transpose solver built by BuildLDLT -# -# FNAME : name of source file to generate - .h and .c files will be made -# N1 : block size (size of outer product matrix) (1..9) -# UNROLL1 : solver inner loop unrolling factor (1..) -# UNROLL2 : factorizer inner loop unrolling factor (1..) -# MADD : if nonzero, generate code for fused multiply-add (0,1) -# FETCH : how to fetch data in the inner loop: -# 0 - load in a batch (the `normal way') -# 1 - delay inner loop loads until just before they're needed - -############################################################################## - -require ("OptimizeUtil"); - -############################################################################## -# optimize solver - -sub testSolver # (filename) -{ - my $filename = $_[0]; - createParametersFile ('ParametersT'); - $params = "$N1 $UNROLL1 $UNROLL2 $MADD $FETCH"; - print "***** TESTING $params\n"; - doit ("rm -f fastltsolve.c fastltsolve.o test_ldlt"); - doit ("make test_ldlt"); - doit ("./test_ldlt t >> $filename"); - open (FILE,">>$filename"); - print FILE " $params\n"; - close FILE; -} - -# find optimal parameters. UNROLL2 has no effect. write results to data5.txt - -open (FILE,">data5.txt"); -print FILE "# solver data from OptimizeLDLT\n"; -close FILE; -$FNAME='fastltsolve'; -$TYPE='t'; -$UNROLL2=1; -for ($N1=1; $N1 <= 5; $N1++) { - for ($UNROLL1=1; $UNROLL1 <= 15; $UNROLL1++) { - for ($MADD=0; $MADD<=1; $MADD++) { - for ($FETCH=0; $FETCH<=1; $FETCH++) { - testSolver ('data5.txt'); - } - } - } -} - -readBackDataFile ('data5.txt'); -createParametersFile ('ParametersT'); diff --git a/extern/ode/dist/ode/fbuild/OptimizeMultidot b/extern/ode/dist/ode/fbuild/OptimizeMultidot deleted file mode 100644 index f2b54383312..00000000000 --- a/extern/ode/dist/ode/fbuild/OptimizeMultidot +++ /dev/null @@ -1,73 +0,0 @@ -#!/usr/bin/perl -# -######################################################################### -# # -# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. # -# All rights reserved. Email: russ@q12.org Web: www.q12.org # -# # -# This library is free software; you can redistribute it and/or # -# modify it under the terms of EITHER: # -# (1) The GNU Lesser General Public License as published by the Free # -# Software Foundation; either version 2.1 of the License, or (at # -# your option) any later version. The text of the GNU Lesser # -# General Public License is included with this library in the # -# file LICENSE.TXT. # -# (2) The BSD-style license that is included with this library in # -# the file LICENSE-BSD.TXT. # -# # -# This library is distributed in the hope that it will be useful, # -# but WITHOUT ANY WARRANTY; without even the implied warranty of # -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files # -# LICENSE.TXT and LICENSE-BSD.TXT for more details. # -# # -######################################################################### - -# optimize the dot product built by BuildMultidot - -############################################################################## - -require ("OptimizeUtil"); - -# multiple -$N1=2; - -# unused standard parameters -$TYPE='unused'; -$UNROLL2=0; # unused -$MADD=0; # unused - -############################################################################## - -sub testMultidot # (filename) -{ - my $filename = $_[0]; - createParametersFile ('ParametersM'); - $params = "$N1 $UNROLL1 $UNROLL2 $MADD $FETCH $LAT1 $LAT2"; - print "***** TESTING $params\n"; - doit ("rm -f fastmultidot.c fastmultidot.o test_multidot"); - doit ("make test_multidot"); - doit ("./test_multidot >> $filename"); - open (FILE,">>$filename"); - print FILE " $params\n"; - close FILE; -} - -# find optimal parameters. write results to data6.txt - -open (FILE,">data6.txt"); -print FILE "# multi-dot product data from OptimizeMultidot\n"; -close FILE; -$FNAME='fastmultidot'; - -for ($UNROLL1=1; $UNROLL1 <= 10; $UNROLL1++) { - for ($LAT1=1; $LAT1 <= 5; $LAT1++) { - for ($LAT2=1; $LAT2 <= 5; $LAT2++) { - for ($FETCH=1; $FETCH<=5; $FETCH++) { - testMultidot ('data6.txt'); - } - } - } -} - -readBackDataFile ('data6.txt'); -createParametersFile ('ParametersM'); diff --git a/extern/ode/dist/ode/fbuild/OptimizeUtil b/extern/ode/dist/ode/fbuild/OptimizeUtil deleted file mode 100644 index 2b882fcba65..00000000000 --- a/extern/ode/dist/ode/fbuild/OptimizeUtil +++ /dev/null @@ -1,86 +0,0 @@ -#!/usr/bin/perl -w -# -######################################################################### -# # -# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. # -# All rights reserved. Email: russ@q12.org Web: www.q12.org # -# # -# This library is free software; you can redistribute it and/or # -# modify it under the terms of EITHER: # -# (1) The GNU Lesser General Public License as published by the Free # -# Software Foundation; either version 2.1 of the License, or (at # -# your option) any later version. The text of the GNU Lesser # -# General Public License is included with this library in the # -# file LICENSE.TXT. # -# (2) The BSD-style license that is included with this library in # -# the file LICENSE-BSD.TXT. # -# # -# This library is distributed in the hope that it will be useful, # -# but WITHOUT ANY WARRANTY; without even the implied warranty of # -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files # -# LICENSE.TXT and LICENSE-BSD.TXT for more details. # -# # -######################################################################### - -package BuildUtil; - - -sub main::doit -{ - my $cmd = $_[0]; - print "$cmd\n"; - system ($cmd)==0 or die "FAILED"; -} - - -sub main::createParametersFile # (filename) -{ - open (PARAM,">$_[0]"); - print PARAM "# perl script to set parameters required by the code generator\n"; - print PARAM "\$FNAME=\"$main::FNAME\";\n" if defined($main::FNAME); - print PARAM "\$TYPE=\"$main::TYPE\";\n" if defined($main::TYPE); - print PARAM "\$N1=$main::N1;\n" if defined($main::N1); - print PARAM "\$UNROLL1=$main::UNROLL1;\n" if defined($main::UNROLL1); - print PARAM "\$UNROLL2=$main::UNROLL2;\n" if defined($main::UNROLL2); - print PARAM "\$MADD=$main::MADD;\n" if defined($main::MADD); - print PARAM "\$FETCH=$main::FETCH;\n" if defined($main::FETCH); - print PARAM "\$LAT1=$main::LAT1;\n" if defined($main::LAT1); - print PARAM "\$LAT2=$main::LAT2;\n" if defined($main::LAT2); - close PARAM; -} - - -# read back a data file and find best parameters - -sub main::readBackDataFile # (filename) -{ - my $filename = $_[0]; - my $maxtime = 1e10; - open (FILE,$filename); - while (<FILE>) { - next if /^\#/; - my $line = lc $_; - if ($line =~ /error/) { - print "ERRORS FOUND IN $filename\n"; - exit 1; - } - $line =~ s/^\s*//; - $line =~ s/\s*$//; - my @nums = split (/\s+/,$line); - $time = $nums[0]; - if ($time < $maxtime) { - $main::N1 = $nums[1]; - $main::UNROLL1 = $nums[2]; - $main::UNROLL2 = $nums[3]; - $main::MADD = $nums[4]; - $main::FETCH = $nums[5]; - $main::LAT1 = $nums[6]; - $main::LAT2 = $nums[7]; - $maxtime = $time; - } - } - close FILE; -} - - -1; diff --git a/extern/ode/dist/ode/fbuild/ParametersD.example b/extern/ode/dist/ode/fbuild/ParametersD.example deleted file mode 100644 index e58f279f7d7..00000000000 --- a/extern/ode/dist/ode/fbuild/ParametersD.example +++ /dev/null @@ -1,32 +0,0 @@ -######################################################################### -# # -# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. # -# All rights reserved. Email: russ@q12.org Web: www.q12.org # -# # -# This library is free software; you can redistribute it and/or # -# modify it under the terms of EITHER: # -# (1) The GNU Lesser General Public License as published by the Free # -# Software Foundation; either version 2.1 of the License, or (at # -# your option) any later version. The text of the GNU Lesser # -# General Public License is included with this library in the # -# file LICENSE.TXT. # -# (2) The BSD-style license that is included with this library in # -# the file LICENSE-BSD.TXT. # -# # -# This library is distributed in the hope that it will be useful, # -# but WITHOUT ANY WARRANTY; without even the implied warranty of # -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files # -# LICENSE.TXT and LICENSE-BSD.TXT for more details. # -# # -######################################################################### - -# perl script to set parameters required by the code generator -$FNAME="fastdot"; -$TYPE="unused"; -$N1=0; -$UNROLL1=2; -$UNROLL2=0; -$MADD=0; -$FETCH=1; -$LAT1=1; -$LAT2=2; diff --git a/extern/ode/dist/ode/fbuild/ParametersF.example b/extern/ode/dist/ode/fbuild/ParametersF.example deleted file mode 100644 index 9881b09ad81..00000000000 --- a/extern/ode/dist/ode/fbuild/ParametersF.example +++ /dev/null @@ -1,30 +0,0 @@ -######################################################################### -# # -# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. # -# All rights reserved. Email: russ@q12.org Web: www.q12.org # -# # -# This library is free software; you can redistribute it and/or # -# modify it under the terms of EITHER: # -# (1) The GNU Lesser General Public License as published by the Free # -# Software Foundation; either version 2.1 of the License, or (at # -# your option) any later version. The text of the GNU Lesser # -# General Public License is included with this library in the # -# file LICENSE.TXT. # -# (2) The BSD-style license that is included with this library in # -# the file LICENSE-BSD.TXT. # -# # -# This library is distributed in the hope that it will be useful, # -# but WITHOUT ANY WARRANTY; without even the implied warranty of # -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files # -# LICENSE.TXT and LICENSE-BSD.TXT for more details. # -# # -######################################################################### - -# perl script to set parameters required by the code generator -$FNAME="fastldlt"; -$TYPE="f"; -$N1=2; -$UNROLL1=2; -$UNROLL2=6; -$MADD=0; -$FETCH=1; diff --git a/extern/ode/dist/ode/fbuild/ParametersM.example b/extern/ode/dist/ode/fbuild/ParametersM.example deleted file mode 100644 index bb81d6b83f1..00000000000 --- a/extern/ode/dist/ode/fbuild/ParametersM.example +++ /dev/null @@ -1,32 +0,0 @@ -######################################################################### -# # -# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. # -# All rights reserved. Email: russ@q12.org Web: www.q12.org # -# # -# This library is free software; you can redistribute it and/or # -# modify it under the terms of EITHER: # -# (1) The GNU Lesser General Public License as published by the Free # -# Software Foundation; either version 2.1 of the License, or (at # -# your option) any later version. The text of the GNU Lesser # -# General Public License is included with this library in the # -# file LICENSE.TXT. # -# (2) The BSD-style license that is included with this library in # -# the file LICENSE-BSD.TXT. # -# # -# This library is distributed in the hope that it will be useful, # -# but WITHOUT ANY WARRANTY; without even the implied warranty of # -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files # -# LICENSE.TXT and LICENSE-BSD.TXT for more details. # -# # -######################################################################### - -# perl script to set parameters required by the code generator -$FNAME="fastmultidot"; -$TYPE="unused"; -$N1=2; -$UNROLL1=1; -$UNROLL2=0; -$MADD=0; -$FETCH=5; -$LAT1=1; -$LAT2=1; diff --git a/extern/ode/dist/ode/fbuild/ParametersS.example b/extern/ode/dist/ode/fbuild/ParametersS.example deleted file mode 100644 index 29c9a91fc16..00000000000 --- a/extern/ode/dist/ode/fbuild/ParametersS.example +++ /dev/null @@ -1,30 +0,0 @@ -######################################################################### -# # -# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. # -# All rights reserved. Email: russ@q12.org Web: www.q12.org # -# # -# This library is free software; you can redistribute it and/or # -# modify it under the terms of EITHER: # -# (1) The GNU Lesser General Public License as published by the Free # -# Software Foundation; either version 2.1 of the License, or (at # -# your option) any later version. The text of the GNU Lesser # -# General Public License is included with this library in the # -# file LICENSE.TXT. # -# (2) The BSD-style license that is included with this library in # -# the file LICENSE-BSD.TXT. # -# # -# This library is distributed in the hope that it will be useful, # -# but WITHOUT ANY WARRANTY; without even the implied warranty of # -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files # -# LICENSE.TXT and LICENSE-BSD.TXT for more details. # -# # -######################################################################### - -# perl script to set parameters required by the code generator -$FNAME="fastlsolve"; -$TYPE="s"; -$N1=4; -$UNROLL1=12; -$UNROLL2=1; -$MADD=1; -$FETCH=0; diff --git a/extern/ode/dist/ode/fbuild/ParametersT.example b/extern/ode/dist/ode/fbuild/ParametersT.example deleted file mode 100644 index 3aa92a31dd2..00000000000 --- a/extern/ode/dist/ode/fbuild/ParametersT.example +++ /dev/null @@ -1,30 +0,0 @@ -######################################################################### -# # -# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. # -# All rights reserved. Email: russ@q12.org Web: www.q12.org # -# # -# This library is free software; you can redistribute it and/or # -# modify it under the terms of EITHER: # -# (1) The GNU Lesser General Public License as published by the Free # -# Software Foundation; either version 2.1 of the License, or (at # -# your option) any later version. The text of the GNU Lesser # -# General Public License is included with this library in the # -# file LICENSE.TXT. # -# (2) The BSD-style license that is included with this library in # -# the file LICENSE-BSD.TXT. # -# # -# This library is distributed in the hope that it will be useful, # -# but WITHOUT ANY WARRANTY; without even the implied warranty of # -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files # -# LICENSE.TXT and LICENSE-BSD.TXT for more details. # -# # -######################################################################### - -# perl script to set parameters required by the code generator -$FNAME="fastltsolve"; -$TYPE="t"; -$N1=4; -$UNROLL1=12; -$UNROLL2=1; -$MADD=1; -$FETCH=0; diff --git a/extern/ode/dist/ode/fbuild/README b/extern/ode/dist/ode/fbuild/README deleted file mode 100644 index bcf7f5aa8b5..00000000000 --- a/extern/ode/dist/ode/fbuild/README +++ /dev/null @@ -1,41 +0,0 @@ - -factorizer/solver builder - -before running `make', copy the following files: - ParametersD.example --> ParametersD - ParametersF.example --> ParametersF - ParametersS.example --> ParametersS - -the files Parameters[D|F|S] don't exist in the CVS archive because -they are changable. - - - -STATS - for chol ------ - -* all with -O1 - -128x128 matrix -atlas = 1724779 clocks -my chol = 1164629 clocks (parameters: 2 2 2 1) with Ai++, Aj++ -my chol = 1140786 clocks (parameters: 2 6 8 0) with Ai++, Aj++ -my chol = 1118968 clocks (parameters: 2 6 8 0) with +ofs - -64x64 matrix -atlas = 374020 clocks -my chol = 157076 clocks (parameters = 2 2 2 1) - -32x32 matrix (12961 flops) -atlas = 83827 clocks -my chol = 25945 clocks (parameters: 2 2 2 1) - - - - -TODO ----- - -* doc! - -* iterate blocks by partial rows to try and keep more data in cache diff --git a/extern/ode/dist/ode/fbuild/ldlt.m b/extern/ode/dist/ode/fbuild/ldlt.m deleted file mode 100644 index 19ae2d1c94e..00000000000 --- a/extern/ode/dist/ode/fbuild/ldlt.m +++ /dev/null @@ -1,26 +0,0 @@ -function [L,d] = ldlt(A) - -n=length(A); -d=zeros(n,1); - -d(1) = 1/A(1,1); -for i=2:n - for j=2:i-1 - A(i,j) = A(i,j) - A(j,1:j-1) * A(i,1:j-1)'; - end - sum = 0; - for j=1:i-1 - q1 = A(i,j); - q2 = q1 * d(j); - A(i,j) = q2; - sum = sum + q1*q2; - end - d(i) = 1/(A(i,i) - sum); -end - -L=A; -for i=1:n - L(i,i:n)=zeros(1,n+1-i); - L(i,i)=1; -end -d = d .\ 1; diff --git a/extern/ode/dist/ode/fbuild/test_dot.cpp b/extern/ode/dist/ode/fbuild/test_dot.cpp deleted file mode 100644 index 62aec32e509..00000000000 --- a/extern/ode/dist/ode/fbuild/test_dot.cpp +++ /dev/null @@ -1,124 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#include <stdio.h> -#include "ode/ode.h" - -#define ALLOCA dALLOCA16 -#define SIZE 1000 - - -// correct dot product, for accuracy testing - -dReal goodDot (dReal *a, dReal *b, int n) -{ - dReal sum=0; - while (n > 0) { - sum += (*a) * (*b); - a++; - b++; - n--; - } - return sum; -} - - -// test dot product accuracy - -void testAccuracy() -{ - // allocate vectors a and b and fill them with random data - dReal *a = (dReal*) ALLOCA (SIZE*sizeof(dReal)); - dReal *b = (dReal*) ALLOCA (SIZE*sizeof(dReal)); - dMakeRandomMatrix (a,1,SIZE,1.0); - dMakeRandomMatrix (b,1,SIZE,1.0); - - for (int n=1; n<100; n++) { - dReal good = goodDot (a,b,n); - dReal test = dDot (a,b,n); - dReal diff = fabs(good-test); - //printf ("diff = %e\n",diff); - if (diff > 1e-10) printf ("ERROR: accuracy test failed\n"); - } -} - - -// test dot product factorizer speed. - -void testSpeed() -{ - // allocate vectors a and b and fill them with random data - dReal *a = (dReal*) ALLOCA (SIZE*sizeof(dReal)); - dReal *b = (dReal*) ALLOCA (SIZE*sizeof(dReal)); - dMakeRandomMatrix (a,1,SIZE,1.0); - dMakeRandomMatrix (b,1,SIZE,1.0); - - // time several dot products, return the minimum timing - double mintime = 1e100; - dStopwatch sw; - for (int i=0; i<1000; i++) { - dStopwatchReset (&sw); - dStopwatchStart (&sw); - - // try a bunch of prime sizes up to 101 - dDot (a,b,2); - dDot (a,b,3); - dDot (a,b,5); - dDot (a,b,7); - dDot (a,b,11); - dDot (a,b,13); - dDot (a,b,17); - dDot (a,b,19); - dDot (a,b,23); - dDot (a,b,29); - dDot (a,b,31); - dDot (a,b,37); - dDot (a,b,41); - dDot (a,b,43); - dDot (a,b,47); - dDot (a,b,53); - dDot (a,b,59); - dDot (a,b,61); - dDot (a,b,67); - dDot (a,b,71); - dDot (a,b,73); - dDot (a,b,79); - dDot (a,b,83); - dDot (a,b,89); - dDot (a,b,97); - dDot (a,b,101); - - dStopwatchStop (&sw); - double time = dStopwatchTime (&sw); - if (time < mintime) mintime = time; - } - - printf ("%.0f",mintime * dTimerTicksPerSecond()); -} - - -int main() -{ - testAccuracy(); - testSpeed(); - return 0; -} diff --git a/extern/ode/dist/ode/fbuild/test_ldlt.cpp b/extern/ode/dist/ode/fbuild/test_ldlt.cpp deleted file mode 100644 index 3b795b9ee7f..00000000000 --- a/extern/ode/dist/ode/fbuild/test_ldlt.cpp +++ /dev/null @@ -1,299 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#include <stdio.h> -#include <malloc.h> -#include "ode/ode.h" - -#define ALLOCA dALLOCA16 - -//**************************************************************************** -// constants - -#ifdef dSINGLE -#define TOL (1e-4) -#else -#define TOL (1e-10) -#endif - -//**************************************************************************** -// test L*X=B solver accuracy. - -void testSolverAccuracy (int n) -{ - int i; - int npad = dPAD(n); - dReal *L = (dReal*) ALLOCA (n*npad*sizeof(dReal)); - dReal *B = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *B2 = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *X = (dReal*) ALLOCA (n*sizeof(dReal)); - - // L is a random lower triangular matrix with 1's on the diagonal - dMakeRandomMatrix (L,n,n,1.0); - dClearUpperTriangle (L,n); - for (i=0; i<n; i++) L[i*npad+i] = 1; - - // B is the right hand side - dMakeRandomMatrix (B,n,1,1.0); - memcpy (X,B,n*sizeof(dReal)); // copy B to X - - dSolveL1 (L,X,n,npad); - - /* - dPrintMatrix (L,n,n); - printf ("\n"); - dPrintMatrix (B,n,1); - printf ("\n"); - dPrintMatrix (X,n,1); - printf ("\n"); - */ - - dSetZero (B2,n); - dMultiply0 (B2,L,X,n,n,1); - dReal error = dMaxDifference (B,B2,1,n); - if (error > TOL) { - printf ("error = %e, size = %d\n",error,n); - } -} - -//**************************************************************************** -// test L^T*X=B solver accuracy. - -void testTransposeSolverAccuracy (int n) -{ - int i; - int npad = dPAD(n); - dReal *L = (dReal*) ALLOCA (n*npad*sizeof(dReal)); - dReal *B = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *B2 = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *X = (dReal*) ALLOCA (n*sizeof(dReal)); - - // L is a random lower triangular matrix with 1's on the diagonal - dMakeRandomMatrix (L,n,n,1.0); - dClearUpperTriangle (L,n); - for (i=0; i<n; i++) L[i*npad+i] = 1; - - // B is the right hand side - dMakeRandomMatrix (B,n,1,1.0); - memcpy (X,B,n*sizeof(dReal)); // copy B to X - - dSolveL1T (L,X,n,npad); - - dSetZero (B2,n); - dMultiply1 (B2,L,X,n,n,1); - dReal error = dMaxDifference (B,B2,1,n); - if (error > TOL) { - printf ("error = %e, size = %d\n",error,n); - } -} - -//**************************************************************************** -// test L*D*L' factorizer accuracy. - -void testLDLTAccuracy (int n) -{ - int i,j; - int npad = dPAD(n); - dReal *A = (dReal*) ALLOCA (n*npad*sizeof(dReal)); - dReal *L = (dReal*) ALLOCA (n*npad*sizeof(dReal)); - dReal *d = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *Atest = (dReal*) ALLOCA (n*npad*sizeof(dReal)); - dReal *DL = (dReal*) ALLOCA (n*npad*sizeof(dReal)); - - dMakeRandomMatrix (A,n,n,1.0); - dMultiply2 (L,A,A,n,n,n); - memcpy (A,L,n*npad*sizeof(dReal)); - dSetZero (d,n); - - dFactorLDLT (L,d,n,npad); - - // make L lower triangular, and convert d into diagonal of D - dClearUpperTriangle (L,n); - for (i=0; i<n; i++) L[i*npad+i] = 1; - for (i=0; i<n; i++) d[i] = 1.0/d[i]; - - // form Atest = L*D*L' - dSetZero (Atest,n*npad); - dSetZero (DL,n*npad); - for (i=0; i<n; i++) { - for (j=0; j<n; j++) DL[i*npad+j] = L[i*npad+j] * d[j]; - } - dMultiply2 (Atest,L,DL,n,n,n); - dReal error = dMaxDifference (A,Atest,n,n); - if (error > TOL) { - printf ("error = %e, size = %d\n",error,n); - } - - /* - printf ("\n"); - dPrintMatrix (A,n,n); - printf ("\n"); - dPrintMatrix (L,n,n); - printf ("\n"); - dPrintMatrix (d,1,n); - */ -} - -//**************************************************************************** -// test L*D*L' factorizer speed. - -void testLDLTSpeed (int n) -{ - int npad = dPAD(n); - - // allocate A - dReal *A = (dReal*) ALLOCA (n*npad*sizeof(dReal)); - - // make B a symmetric positive definite matrix - dMakeRandomMatrix (A,n,n,1.0); - dReal *B = (dReal*) ALLOCA (n*npad*sizeof(dReal)); - dSetZero (B,n*npad); - dMultiply2 (B,A,A,n,n,n); - - // make d - dReal *d = (dReal*) ALLOCA (n*sizeof(dReal)); - dSetZero (d,n); - - // time several factorizations, return the minimum timing - double mintime = 1e100; - dStopwatch sw; - for (int i=0; i<100; i++) { - memcpy (A,B,n*npad*sizeof(dReal)); - dStopwatchReset (&sw); - dStopwatchStart (&sw); - - dFactorLDLT (A,d,n,npad); - - dStopwatchStop (&sw); - double time = dStopwatchTime (&sw); - if (time < mintime) mintime = time; - } - - printf ("%.0f",mintime * dTimerTicksPerSecond()); -} - -//**************************************************************************** -// test solver speed. - -void testSolverSpeed (int n, int transpose) -{ - int i; - int npad = dPAD(n); - - // allocate L,B,X - dReal *L = (dReal*) ALLOCA (n*npad*sizeof(dReal)); - dReal *B = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *X = (dReal*) ALLOCA (n*sizeof(dReal)); - - // L is a random lower triangular matrix with 1's on the diagonal - dMakeRandomMatrix (L,n,n,1.0); - dClearUpperTriangle (L,n); - for (i=0; i<n; i++) L[i*npad+i] = 1; - - // B is the right hand side - dMakeRandomMatrix (B,n,1,1.0); - - // time several factorizations, return the minimum timing - double mintime = 1e100; - dStopwatch sw; - for (int i=0; i<100; i++) { - memcpy (X,B,n*sizeof(dReal)); // copy B to X - - if (transpose) { - dStopwatchReset (&sw); - dStopwatchStart (&sw); - dSolveL1T (L,X,n,npad); - dStopwatchStop (&sw); - } - else { - dStopwatchReset (&sw); - dStopwatchStart (&sw); - dSolveL1 (L,X,n,npad); - dStopwatchStop (&sw); - } - - double time = dStopwatchTime (&sw); - if (time < mintime) mintime = time; - } - - printf ("%.0f",mintime * dTimerTicksPerSecond()); -} - -//**************************************************************************** -// the single command line argument is 'f' to test and time the factorizer, -// or 's' to test and time the solver. - - -void testAccuracy (int n, char type) -{ - if (type == 'f') testLDLTAccuracy (n); - if (type == 's') testSolverAccuracy (n); - if (type == 't') testTransposeSolverAccuracy (n); -} - - -void testSpeed (int n, char type) -{ - if (type == 'f') testLDLTSpeed (n); - if (type == 's') testSolverSpeed (n,0); - if (type == 't') testSolverSpeed (n,1); -} - - -int main (int argc, char **argv) -{ - if (argc != 2 || argv[1][0] == 0 || argv[1][1] != 0 || - (argv[1][0] != 'f' && argv[1][0] != 's' && argv[1][0] != 't')) { - fprintf (stderr,"Usage: test_ldlt [f|s|t]\n"); - exit (1); - } - char type = argv[1][0]; - - // accuracy test: test all sizes up to 20 then all prime sizes up to 101 - int i; - for (i=1; i<20; i++) { - testAccuracy (i,type); - } - testAccuracy (23,type); - testAccuracy (29,type); - testAccuracy (31,type); - testAccuracy (37,type); - testAccuracy (41,type); - testAccuracy (43,type); - testAccuracy (47,type); - testAccuracy (53,type); - testAccuracy (59,type); - testAccuracy (61,type); - testAccuracy (67,type); - testAccuracy (71,type); - testAccuracy (73,type); - testAccuracy (79,type); - testAccuracy (83,type); - testAccuracy (89,type); - testAccuracy (97,type); - testAccuracy (101,type); - - // test speed on a 127x127 matrix - testSpeed (127,type); - - return 0; -} diff --git a/extern/ode/dist/ode/fbuild/test_multidot.cpp b/extern/ode/dist/ode/fbuild/test_multidot.cpp deleted file mode 100644 index 0e961152fed..00000000000 --- a/extern/ode/dist/ode/fbuild/test_multidot.cpp +++ /dev/null @@ -1,144 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#include <stdio.h> -#include "ode/ode.h" - -#define NUM_A 2 -#define ALLOCA dALLOCA16 -#define SIZE 1000 - - -extern "C" void dMultidot2 (const dReal *a0, const dReal *a1, - const dReal *b, dReal *outsum, int n); -/* -extern "C" void dMultidot4 (const dReal *a0, const dReal *a1, - const dReal *a2, const dReal *a3, - const dReal *b, dReal *outsum, int n); -*/ - - -// correct dot product, for accuracy testing - -dReal goodDot (dReal *a, dReal *b, int n) -{ - dReal sum=0; - while (n > 0) { - sum += (*a) * (*b); - a++; - b++; - n--; - } - return sum; -} - - -// test multi-dot product accuracy - -void testAccuracy() -{ - int j; - - // allocate vectors a and b and fill them with random data - dReal *a[NUM_A]; - for (j=0; j<NUM_A; j++) a[j] = (dReal*) ALLOCA (SIZE*sizeof(dReal)); - dReal *b = (dReal*) ALLOCA (SIZE*sizeof(dReal)); - for (j=0; j<NUM_A; j++) dMakeRandomMatrix (a[j],1,SIZE,1.0); - dMakeRandomMatrix (b,1,SIZE,1.0); - - for (int n=1; n<100; n++) { - dReal good[NUM_A]; - for (j=0; j<NUM_A; j++) good[j] = goodDot (a[j],b,n); - dReal test[4]; - dMultidot2 (a[0],a[1],b,test,n); - dReal diff = 0; - for (j=0; j<NUM_A; j++) diff += fabs(good[j]-test[j]); - // printf ("diff = %e\n",diff); - if (diff > 1e-10) printf ("ERROR: accuracy test failed\n"); - } -} - - -// test multi-dot product factorizer speed. - -void testSpeed() -{ - int j; - dReal sum[NUM_A]; - - // allocate vectors a and b and fill them with random data - dReal *a[NUM_A]; - for (j=0; j<NUM_A; j++) a[j] = (dReal*) ALLOCA (SIZE*sizeof(dReal)); - dReal *b = (dReal*) ALLOCA (SIZE*sizeof(dReal)); - for (j=0; j<NUM_A; j++) dMakeRandomMatrix (a[j],1,SIZE,1.0); - dMakeRandomMatrix (b,1,SIZE,1.0); - - // time several dot products, return the minimum timing - double mintime = 1e100; - dStopwatch sw; - for (int i=0; i<1000; i++) { - dStopwatchReset (&sw); - dStopwatchStart (&sw); - - // try a bunch of prime sizes up to 101 - dMultidot2 (a[0],a[1],b,sum,2); - dMultidot2 (a[0],a[1],b,sum,3); - dMultidot2 (a[0],a[1],b,sum,5); - dMultidot2 (a[0],a[1],b,sum,7); - dMultidot2 (a[0],a[1],b,sum,11); - dMultidot2 (a[0],a[1],b,sum,13); - dMultidot2 (a[0],a[1],b,sum,17); - dMultidot2 (a[0],a[1],b,sum,19); - dMultidot2 (a[0],a[1],b,sum,23); - dMultidot2 (a[0],a[1],b,sum,29); - dMultidot2 (a[0],a[1],b,sum,31); - dMultidot2 (a[0],a[1],b,sum,37); - dMultidot2 (a[0],a[1],b,sum,41); - dMultidot2 (a[0],a[1],b,sum,43); - dMultidot2 (a[0],a[1],b,sum,47); - dMultidot2 (a[0],a[1],b,sum,53); - dMultidot2 (a[0],a[1],b,sum,59); - dMultidot2 (a[0],a[1],b,sum,61); - dMultidot2 (a[0],a[1],b,sum,67); - dMultidot2 (a[0],a[1],b,sum,71); - dMultidot2 (a[0],a[1],b,sum,73); - dMultidot2 (a[0],a[1],b,sum,79); - dMultidot2 (a[0],a[1],b,sum,83); - dMultidot2 (a[0],a[1],b,sum,89); - dMultidot2 (a[0],a[1],b,sum,97); - dMultidot2 (a[0],a[1],b,sum,101); - - dStopwatchStop (&sw); - double time = dStopwatchTime (&sw); - if (time < mintime) mintime = time; - } - - printf ("%.0f",mintime * dTimerTicksPerSecond()); -} - - -int main() -{ - testAccuracy(); - testSpeed(); - return 0; -} diff --git a/extern/ode/dist/ode/src/array.cpp b/extern/ode/dist/ode/src/array.cpp deleted file mode 100644 index cbb1a6ed557..00000000000 --- a/extern/ode/dist/ode/src/array.cpp +++ /dev/null @@ -1,80 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#include <ode/config.h> -#include <ode/memory.h> -#include <ode/error.h> -#include "array.h" - - -static inline int roundUpToPowerOfTwo (int x) -{ - int i = 1; - while (i < x) i <<= 1; - return i; -} - - -void dArrayBase::_freeAll (int sizeofT) -{ - if (_data) { - if (_data == this+1) return; // if constructLocalArray() was called - dFree (_data,_anum * sizeofT); - } -} - - -void dArrayBase::_setSize (int newsize, int sizeofT) -{ - if (newsize < 0) return; - if (newsize > _anum) { - if (_data == this+1) { - // this is a no-no, because constructLocalArray() was called - dDebug (0,"setSize() out of space in LOCAL array"); - } - int newanum = roundUpToPowerOfTwo (newsize); - if (_data) _data = dRealloc (_data, _anum*sizeofT, newanum*sizeofT); - else _data = dAlloc (newanum*sizeofT); - _anum = newanum; - } - _size = newsize; -} - - -void * dArrayBase::operator new (size_t size) -{ - return dAlloc (size); -} - - -void dArrayBase::operator delete (void *ptr, size_t size) -{ - dFree (ptr,size); -} - - -void dArrayBase::constructLocalArray (int __anum) -{ - _size = 0; - _anum = __anum; - _data = this+1; -} diff --git a/extern/ode/dist/ode/src/array.h b/extern/ode/dist/ode/src/array.h deleted file mode 100644 index 97c2cebc2f4..00000000000 --- a/extern/ode/dist/ode/src/array.h +++ /dev/null @@ -1,135 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -/* this comes from the `reuse' library. copy any changes back to the source. - * - * Variable sized array template. The array is always stored in a contiguous - * chunk. The array can be resized. A size increase will cause more memory - * to be allocated, and may result in relocation of the array memory. - * A size decrease has no effect on the memory allocation. - * - * Array elements with constructors or destructors are not supported! - * But if you must have such elements, here's what to know/do: - * - Bitwise copy is used when copying whole arrays. - * - When copying individual items (via push(), insert() etc) the `=' - * (equals) operator is used. Thus you should define this operator to do - * a bitwise copy. You should probably also define the copy constructor. - */ - - -#ifndef _ODE_ARRAY_H_ -#define _ODE_ARRAY_H_ - -#include <ode/config.h> - - -// this base class has no constructors or destructor, for your convenience. - -class dArrayBase { -protected: - int _size; // number of elements in `data' - int _anum; // allocated number of elements in `data' - void *_data; // array data - - void _freeAll (int sizeofT); - void _setSize (int newsize, int sizeofT); - // set the array size to `newsize', allocating more memory if necessary. - // if newsize>_anum and is a power of two then this is guaranteed to - // set _size and _anum to newsize. - -public: - // not: dArrayBase () { _size=0; _anum=0; _data=0; } - - int size() const { return _size; } - int allocatedSize() const { return _anum; } - void * operator new (size_t size); - void operator delete (void *ptr, size_t size); - - void constructor() { _size=0; _anum=0; _data=0; } - // if this structure is allocated with malloc() instead of new, you can - // call this to set it up. - - void constructLocalArray (int __anum); - // this helper function allows non-reallocating arrays to be constructed - // on the stack (or in the heap if necessary). this is something of a - // kludge and should be used with extreme care. this function acts like - // a constructor - it is called on uninitialized memory that will hold the - // Array structure and the data. __anum is the number of elements that - // are allocated. the memory MUST be allocated with size: - // sizeof(ArrayBase) + __anum*sizeof(T) - // arrays allocated this way will never try to reallocate or free the - // memory - that's your job. -}; - - -template <class T> class dArray : public dArrayBase { -public: - void equals (const dArray<T> &x) { - setSize (x.size()); - memcpy (_data,x._data,x._size * sizeof(T)); - } - - dArray () { constructor(); } - dArray (const dArray<T> &x) { constructor(); equals (x); } - ~dArray () { _freeAll(sizeof(T)); } - void setSize (int newsize) { _setSize (newsize,sizeof(T)); } - T *data() const { return (T*) _data; } - T & operator[] (int i) const { return ((T*)_data)[i]; } - void operator = (const dArray<T> &x) { equals (x); } - - void push (const T item) { - if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T)); - ((T*)_data)[_size-1] = item; - } - - void swap (dArray<T> &x) { - int tmp1; - void *tmp2; - tmp1=_size; _size=x._size; x._size=tmp1; - tmp1=_anum; _anum=x._anum; x._anum=tmp1; - tmp2=_data; _data=x._data; x._data=tmp2; - } - - // insert the item at the position `i'. if i<0 then add the item to the - // start, if i >= size then add the item to the end of the array. - void insert (int i, const T item) { - if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T)); - if (i >= (_size-1)) i = _size-1; // add to end - else { - if (i < 0) i=0; // add to start - int n = _size-1-i; - if (n>0) memmove (((T*)_data) + i+1, ((T*)_data) + i, n*sizeof(T)); - } - ((T*)_data)[i] = item; - } - - void remove (int i) { - if (i >= 0 && i < _size) { // passing this test guarantees size>0 - int n = _size-1-i; - if (n>0) memmove (((T*)_data) + i, ((T*)_data) + i+1, n*sizeof(T)); - _size--; - } - } -}; - -#endif - diff --git a/extern/ode/dist/ode/src/error.cpp b/extern/ode/dist/ode/src/error.cpp deleted file mode 100644 index 9b33db55f0c..00000000000 --- a/extern/ode/dist/ode/src/error.cpp +++ /dev/null @@ -1,172 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#include <ode/config.h> -#include <ode/error.h> - - -static dMessageFunction *error_function = 0; -static dMessageFunction *debug_function = 0; -static dMessageFunction *message_function = 0; - - -extern "C" void dSetErrorHandler (dMessageFunction *fn) -{ - error_function = fn; -} - - -extern "C" void dSetDebugHandler (dMessageFunction *fn) -{ - debug_function = fn; -} - - -extern "C" void dSetMessageHandler (dMessageFunction *fn) -{ - message_function = fn; -} - - -extern "C" dMessageFunction *dGetErrorHandler() -{ - return error_function; -} - - -extern "C" dMessageFunction *dGetDebugHandler() -{ - return debug_function; -} - - -extern "C" dMessageFunction *dGetMessageHandler() -{ - return message_function; -} - - -static void printMessage (int num, const char *msg1, const char *msg2, - va_list ap) -{ - fflush (stderr); - fflush (stdout); - if (num) fprintf (stderr,"\n%s %d: ",msg1,num); - else fprintf (stderr,"\n%s: ",msg1); - vfprintf (stderr,msg2,ap); - fprintf (stderr,"\n"); - fflush (stderr); -} - -//**************************************************************************** -// unix - -#ifndef WIN32 - -extern "C" void dError (int num, const char *msg, ...) -{ - va_list ap; - va_start (ap,msg); - if (error_function) error_function (num,msg,ap); - else printMessage (num,"ODE Error",msg,ap); - exit (1); -} - - -extern "C" void dDebug (int num, const char *msg, ...) -{ - va_list ap; - va_start (ap,msg); - if (debug_function) debug_function (num,msg,ap); - else printMessage (num,"ODE INTERNAL ERROR",msg,ap); - // *((char *)0) = 0; ... commit SEGVicide - abort(); -} - - -extern "C" void dMessage (int num, const char *msg, ...) -{ - va_list ap; - va_start (ap,msg); - if (message_function) message_function (num,msg,ap); - else printMessage (num,"ODE Message",msg,ap); -} - -#endif - -//**************************************************************************** -// windows - -#ifdef WIN32 - -// isn't cygwin annoying! -#ifdef CYGWIN -#define _snprintf snprintf -#define _vsnprintf vsnprintf -#endif - - -#include "windows.h" - - -extern "C" void dError (int num, const char *msg, ...) -{ - va_list ap; - va_start (ap,msg); - if (error_function) error_function (num,msg,ap); - else { - char s[1000],title[100]; - _snprintf (title,sizeof(title),"ODE Error %d",num); - _vsnprintf (s,sizeof(s),msg,ap); - s[sizeof(s)-1] = 0; - MessageBox(0,s,title,MB_OK | MB_ICONWARNING); - } - exit (1); -} - - -extern "C" void dDebug (int num, const char *msg, ...) -{ - va_list ap; - va_start (ap,msg); - if (debug_function) debug_function (num,msg,ap); - else { - char s[1000],title[100]; - _snprintf (title,sizeof(title),"ODE INTERNAL ERROR %d",num); - _vsnprintf (s,sizeof(s),msg,ap); - s[sizeof(s)-1] = 0; - MessageBox(0,s,title,MB_OK | MB_ICONSTOP); - } - abort(); -} - - -extern "C" void dMessage (int num, const char *msg, ...) -{ - va_list ap; - va_start (ap,msg); - if (message_function) message_function (num,msg,ap); - else printMessage (num,"ODE Message",msg,ap); -} - - -#endif diff --git a/extern/ode/dist/ode/src/fastdot.c b/extern/ode/dist/ode/src/fastdot.c deleted file mode 100644 index 148d2dd9e17..00000000000 --- a/extern/ode/dist/ode/src/fastdot.c +++ /dev/null @@ -1,30 +0,0 @@ -/* generated code, do not edit. */ - -#include "ode/matrix.h" - - -dReal dDot (const dReal *a, const dReal *b, int n) -{ - dReal p0,q0,m0,p1,q1,m1,sum; - sum = 0; - n -= 2; - while (n >= 0) { - p0 = a[0]; q0 = b[0]; - m0 = p0 * q0; - p1 = a[1]; q1 = b[1]; - m1 = p1 * q1; - sum += m0; - sum += m1; - a += 2; - b += 2; - n -= 2; - } - n += 2; - while (n > 0) { - sum += (*a) * (*b); - a++; - b++; - n--; - } - return sum; -} diff --git a/extern/ode/dist/ode/src/fastldlt.c b/extern/ode/dist/ode/src/fastldlt.c deleted file mode 100644 index df2ea6ec229..00000000000 --- a/extern/ode/dist/ode/src/fastldlt.c +++ /dev/null @@ -1,381 +0,0 @@ -/* generated code, do not edit. */ - -#include "ode/matrix.h" - -/* solve L*X=B, with B containing 1 right hand sides. - * L is an n*n lower triangular matrix with ones on the diagonal. - * L is stored by rows and its leading dimension is lskip. - * B is an n*1 matrix that contains the right hand sides. - * B is stored by columns and its leading dimension is also lskip. - * B is overwritten with X. - * this processes blocks of 2*2. - * if this is in the factorizer source file, n must be a multiple of 2. - */ - -static void dSolveL1_1 (const dReal *L, dReal *B, int n, int lskip1) -{ - /* declare variables - Z matrix, p and q vectors, etc */ - dReal Z11,m11,Z21,m21,p1,q1,p2,*ex; - const dReal *ell; - int i,j; - /* compute all 2 x 1 blocks of X */ - for (i=0; i < n; i+=2) { - /* compute all 2 x 1 block of X, from rows i..i+2-1 */ - /* set the Z matrix to 0 */ - Z11=0; - Z21=0; - ell = L + i*lskip1; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-2; j >= 0; j -= 2) { - /* compute outer product and add it to the Z matrix */ - p1=ell[0]; - q1=ex[0]; - m11 = p1 * q1; - p2=ell[lskip1]; - m21 = p2 * q1; - Z11 += m11; - Z21 += m21; - /* compute outer product and add it to the Z matrix */ - p1=ell[1]; - q1=ex[1]; - m11 = p1 * q1; - p2=ell[1+lskip1]; - m21 = p2 * q1; - /* advance pointers */ - ell += 2; - ex += 2; - Z11 += m11; - Z21 += m21; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 2; - for (; j > 0; j--) { - /* compute outer product and add it to the Z matrix */ - p1=ell[0]; - q1=ex[0]; - m11 = p1 * q1; - p2=ell[lskip1]; - m21 = p2 * q1; - /* advance pointers */ - ell += 1; - ex += 1; - Z11 += m11; - Z21 += m21; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - p1 = ell[lskip1]; - Z21 = ex[1] - Z21 - p1*Z11; - ex[1] = Z21; - /* end of outer loop */ - } -} - -/* solve L*X=B, with B containing 2 right hand sides. - * L is an n*n lower triangular matrix with ones on the diagonal. - * L is stored by rows and its leading dimension is lskip. - * B is an n*2 matrix that contains the right hand sides. - * B is stored by columns and its leading dimension is also lskip. - * B is overwritten with X. - * this processes blocks of 2*2. - * if this is in the factorizer source file, n must be a multiple of 2. - */ - -static void dSolveL1_2 (const dReal *L, dReal *B, int n, int lskip1) -{ - /* declare variables - Z matrix, p and q vectors, etc */ - dReal Z11,m11,Z12,m12,Z21,m21,Z22,m22,p1,q1,p2,q2,*ex; - const dReal *ell; - int i,j; - /* compute all 2 x 2 blocks of X */ - for (i=0; i < n; i+=2) { - /* compute all 2 x 2 block of X, from rows i..i+2-1 */ - /* set the Z matrix to 0 */ - Z11=0; - Z12=0; - Z21=0; - Z22=0; - ell = L + i*lskip1; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-2; j >= 0; j -= 2) { - /* compute outer product and add it to the Z matrix */ - p1=ell[0]; - q1=ex[0]; - m11 = p1 * q1; - q2=ex[lskip1]; - m12 = p1 * q2; - p2=ell[lskip1]; - m21 = p2 * q1; - m22 = p2 * q2; - Z11 += m11; - Z12 += m12; - Z21 += m21; - Z22 += m22; - /* compute outer product and add it to the Z matrix */ - p1=ell[1]; - q1=ex[1]; - m11 = p1 * q1; - q2=ex[1+lskip1]; - m12 = p1 * q2; - p2=ell[1+lskip1]; - m21 = p2 * q1; - m22 = p2 * q2; - /* advance pointers */ - ell += 2; - ex += 2; - Z11 += m11; - Z12 += m12; - Z21 += m21; - Z22 += m22; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 2; - for (; j > 0; j--) { - /* compute outer product and add it to the Z matrix */ - p1=ell[0]; - q1=ex[0]; - m11 = p1 * q1; - q2=ex[lskip1]; - m12 = p1 * q2; - p2=ell[lskip1]; - m21 = p2 * q1; - m22 = p2 * q2; - /* advance pointers */ - ell += 1; - ex += 1; - Z11 += m11; - Z12 += m12; - Z21 += m21; - Z22 += m22; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - Z12 = ex[lskip1] - Z12; - ex[lskip1] = Z12; - p1 = ell[lskip1]; - Z21 = ex[1] - Z21 - p1*Z11; - ex[1] = Z21; - Z22 = ex[1+lskip1] - Z22 - p1*Z12; - ex[1+lskip1] = Z22; - /* end of outer loop */ - } -} - - -void dFactorLDLT (dReal *A, dReal *d, int n, int nskip1) -{ - int i,j; - dReal sum,*ell,*dee,dd,p1,p2,q1,q2,Z11,m11,Z21,m21,Z22,m22; - if (n < 1) return; - - for (i=0; i<=n-2; i += 2) { - /* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */ - dSolveL1_2 (A,A+i*nskip1,i,nskip1); - /* scale the elements in a 2 x i block at A(i,0), and also */ - /* compute Z = the outer product matrix that we'll need. */ - Z11 = 0; - Z21 = 0; - Z22 = 0; - ell = A+i*nskip1; - dee = d; - for (j=i-6; j >= 0; j -= 6) { - p1 = ell[0]; - p2 = ell[nskip1]; - dd = dee[0]; - q1 = p1*dd; - q2 = p2*dd; - ell[0] = q1; - ell[nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - p1 = ell[1]; - p2 = ell[1+nskip1]; - dd = dee[1]; - q1 = p1*dd; - q2 = p2*dd; - ell[1] = q1; - ell[1+nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - p1 = ell[2]; - p2 = ell[2+nskip1]; - dd = dee[2]; - q1 = p1*dd; - q2 = p2*dd; - ell[2] = q1; - ell[2+nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - p1 = ell[3]; - p2 = ell[3+nskip1]; - dd = dee[3]; - q1 = p1*dd; - q2 = p2*dd; - ell[3] = q1; - ell[3+nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - p1 = ell[4]; - p2 = ell[4+nskip1]; - dd = dee[4]; - q1 = p1*dd; - q2 = p2*dd; - ell[4] = q1; - ell[4+nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - p1 = ell[5]; - p2 = ell[5+nskip1]; - dd = dee[5]; - q1 = p1*dd; - q2 = p2*dd; - ell[5] = q1; - ell[5+nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - ell += 6; - dee += 6; - } - /* compute left-over iterations */ - j += 6; - for (; j > 0; j--) { - p1 = ell[0]; - p2 = ell[nskip1]; - dd = dee[0]; - q1 = p1*dd; - q2 = p2*dd; - ell[0] = q1; - ell[nskip1] = q2; - m11 = p1*q1; - m21 = p2*q1; - m22 = p2*q2; - Z11 += m11; - Z21 += m21; - Z22 += m22; - ell++; - dee++; - } - /* solve for diagonal 2 x 2 block at A(i,i) */ - Z11 = ell[0] - Z11; - Z21 = ell[nskip1] - Z21; - Z22 = ell[1+nskip1] - Z22; - dee = d + i; - /* factorize 2 x 2 block Z,dee */ - /* factorize row 1 */ - dee[0] = dRecip(Z11); - /* factorize row 2 */ - sum = 0; - q1 = Z21; - q2 = q1 * dee[0]; - Z21 = q2; - sum += q1*q2; - dee[1] = dRecip(Z22 - sum); - /* done factorizing 2 x 2 block */ - ell[nskip1] = Z21; - } - /* compute the (less than 2) rows at the bottom */ - switch (n-i) { - case 0: - break; - - case 1: - dSolveL1_1 (A,A+i*nskip1,i,nskip1); - /* scale the elements in a 1 x i block at A(i,0), and also */ - /* compute Z = the outer product matrix that we'll need. */ - Z11 = 0; - ell = A+i*nskip1; - dee = d; - for (j=i-6; j >= 0; j -= 6) { - p1 = ell[0]; - dd = dee[0]; - q1 = p1*dd; - ell[0] = q1; - m11 = p1*q1; - Z11 += m11; - p1 = ell[1]; - dd = dee[1]; - q1 = p1*dd; - ell[1] = q1; - m11 = p1*q1; - Z11 += m11; - p1 = ell[2]; - dd = dee[2]; - q1 = p1*dd; - ell[2] = q1; - m11 = p1*q1; - Z11 += m11; - p1 = ell[3]; - dd = dee[3]; - q1 = p1*dd; - ell[3] = q1; - m11 = p1*q1; - Z11 += m11; - p1 = ell[4]; - dd = dee[4]; - q1 = p1*dd; - ell[4] = q1; - m11 = p1*q1; - Z11 += m11; - p1 = ell[5]; - dd = dee[5]; - q1 = p1*dd; - ell[5] = q1; - m11 = p1*q1; - Z11 += m11; - ell += 6; - dee += 6; - } - /* compute left-over iterations */ - j += 6; - for (; j > 0; j--) { - p1 = ell[0]; - dd = dee[0]; - q1 = p1*dd; - ell[0] = q1; - m11 = p1*q1; - Z11 += m11; - ell++; - dee++; - } - /* solve for diagonal 1 x 1 block at A(i,i) */ - Z11 = ell[0] - Z11; - dee = d + i; - /* factorize 1 x 1 block Z,dee */ - /* factorize row 1 */ - dee[0] = dRecip(Z11); - /* done factorizing 1 x 1 block */ - break; - - default: *((char*)0)=0; /* this should never happen! */ - } -} diff --git a/extern/ode/dist/ode/src/fastlsolve.c b/extern/ode/dist/ode/src/fastlsolve.c deleted file mode 100644 index 0ae99d62d0b..00000000000 --- a/extern/ode/dist/ode/src/fastlsolve.c +++ /dev/null @@ -1,298 +0,0 @@ -/* generated code, do not edit. */ - -#include "ode/matrix.h" - -/* solve L*X=B, with B containing 1 right hand sides. - * L is an n*n lower triangular matrix with ones on the diagonal. - * L is stored by rows and its leading dimension is lskip. - * B is an n*1 matrix that contains the right hand sides. - * B is stored by columns and its leading dimension is also lskip. - * B is overwritten with X. - * this processes blocks of 4*4. - * if this is in the factorizer source file, n must be a multiple of 4. - */ - -void dSolveL1 (const dReal *L, dReal *B, int n, int lskip1) -{ - /* declare variables - Z matrix, p and q vectors, etc */ - dReal Z11,Z21,Z31,Z41,p1,q1,p2,p3,p4,*ex; - const dReal *ell; - int lskip2,lskip3,i,j; - /* compute lskip values */ - lskip2 = 2*lskip1; - lskip3 = 3*lskip1; - /* compute all 4 x 1 blocks of X */ - for (i=0; i <= n-4; i+=4) { - /* compute all 4 x 1 block of X, from rows i..i+4-1 */ - /* set the Z matrix to 0 */ - Z11=0; - Z21=0; - Z31=0; - Z41=0; - ell = L + i*lskip1; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-12; j >= 0; j -= 12) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - p2=ell[lskip1]; - p3=ell[lskip2]; - p4=ell[lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[1]; - q1=ex[1]; - p2=ell[1+lskip1]; - p3=ell[1+lskip2]; - p4=ell[1+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[2]; - q1=ex[2]; - p2=ell[2+lskip1]; - p3=ell[2+lskip2]; - p4=ell[2+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[3]; - q1=ex[3]; - p2=ell[3+lskip1]; - p3=ell[3+lskip2]; - p4=ell[3+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[4]; - q1=ex[4]; - p2=ell[4+lskip1]; - p3=ell[4+lskip2]; - p4=ell[4+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[5]; - q1=ex[5]; - p2=ell[5+lskip1]; - p3=ell[5+lskip2]; - p4=ell[5+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[6]; - q1=ex[6]; - p2=ell[6+lskip1]; - p3=ell[6+lskip2]; - p4=ell[6+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[7]; - q1=ex[7]; - p2=ell[7+lskip1]; - p3=ell[7+lskip2]; - p4=ell[7+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[8]; - q1=ex[8]; - p2=ell[8+lskip1]; - p3=ell[8+lskip2]; - p4=ell[8+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[9]; - q1=ex[9]; - p2=ell[9+lskip1]; - p3=ell[9+lskip2]; - p4=ell[9+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[10]; - q1=ex[10]; - p2=ell[10+lskip1]; - p3=ell[10+lskip2]; - p4=ell[10+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* load p and q values */ - p1=ell[11]; - q1=ex[11]; - p2=ell[11+lskip1]; - p3=ell[11+lskip2]; - p4=ell[11+lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* advance pointers */ - ell += 12; - ex += 12; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 12; - for (; j > 0; j--) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - p2=ell[lskip1]; - p3=ell[lskip2]; - p4=ell[lskip3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - Z21 += p2 * q1; - Z31 += p3 * q1; - Z41 += p4 * q1; - /* advance pointers */ - ell += 1; - ex += 1; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - p1 = ell[lskip1]; - Z21 = ex[1] - Z21 - p1*Z11; - ex[1] = Z21; - p1 = ell[lskip2]; - p2 = ell[1+lskip2]; - Z31 = ex[2] - Z31 - p1*Z11 - p2*Z21; - ex[2] = Z31; - p1 = ell[lskip3]; - p2 = ell[1+lskip3]; - p3 = ell[2+lskip3]; - Z41 = ex[3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31; - ex[3] = Z41; - /* end of outer loop */ - } - /* compute rows at end that are not a multiple of block size */ - for (; i < n; i++) { - /* compute all 1 x 1 block of X, from rows i..i+1-1 */ - /* set the Z matrix to 0 */ - Z11=0; - ell = L + i*lskip1; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-12; j >= 0; j -= 12) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[1]; - q1=ex[1]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[2]; - q1=ex[2]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[3]; - q1=ex[3]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[4]; - q1=ex[4]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[5]; - q1=ex[5]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[6]; - q1=ex[6]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[7]; - q1=ex[7]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[8]; - q1=ex[8]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[9]; - q1=ex[9]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[10]; - q1=ex[10]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* load p and q values */ - p1=ell[11]; - q1=ex[11]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* advance pointers */ - ell += 12; - ex += 12; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 12; - for (; j > 0; j--) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - /* compute outer product and add it to the Z matrix */ - Z11 += p1 * q1; - /* advance pointers */ - ell += 1; - ex += 1; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - } -} diff --git a/extern/ode/dist/ode/src/fastltsolve.c b/extern/ode/dist/ode/src/fastltsolve.c deleted file mode 100644 index eb950f6076a..00000000000 --- a/extern/ode/dist/ode/src/fastltsolve.c +++ /dev/null @@ -1,199 +0,0 @@ -/* generated code, do not edit. */ - -#include "ode/matrix.h" - -/* solve L^T * x=b, with b containing 1 right hand side. - * L is an n*n lower triangular matrix with ones on the diagonal. - * L is stored by rows and its leading dimension is lskip. - * b is an n*1 matrix that contains the right hand side. - * b is overwritten with x. - * this processes blocks of 4. - */ - -void dSolveL1T (const dReal *L, dReal *B, int n, int lskip1) -{ - /* declare variables - Z matrix, p and q vectors, etc */ - dReal Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex; - const dReal *ell; - int lskip2,lskip3,i,j; - /* special handling for L and B because we're solving L1 *transpose* */ - L = L + (n-1)*(lskip1+1); - B = B + n-1; - lskip1 = -lskip1; - /* compute lskip values */ - lskip2 = 2*lskip1; - lskip3 = 3*lskip1; - /* compute all 4 x 1 blocks of X */ - for (i=0; i <= n-4; i+=4) { - /* compute all 4 x 1 block of X, from rows i..i+4-1 */ - /* set the Z matrix to 0 */ - Z11=0; - Z21=0; - Z31=0; - Z41=0; - ell = L - i; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-4; j >= 0; j -= 4) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - p2=ell[-1]; - p3=ell[-2]; - p4=ell[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - m21 = p2 * q1; - m31 = p3 * q1; - m41 = p4 * q1; - ell += lskip1; - Z11 += m11; - Z21 += m21; - Z31 += m31; - Z41 += m41; - /* load p and q values */ - p1=ell[0]; - q1=ex[-1]; - p2=ell[-1]; - p3=ell[-2]; - p4=ell[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - m21 = p2 * q1; - m31 = p3 * q1; - m41 = p4 * q1; - ell += lskip1; - Z11 += m11; - Z21 += m21; - Z31 += m31; - Z41 += m41; - /* load p and q values */ - p1=ell[0]; - q1=ex[-2]; - p2=ell[-1]; - p3=ell[-2]; - p4=ell[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - m21 = p2 * q1; - m31 = p3 * q1; - m41 = p4 * q1; - ell += lskip1; - Z11 += m11; - Z21 += m21; - Z31 += m31; - Z41 += m41; - /* load p and q values */ - p1=ell[0]; - q1=ex[-3]; - p2=ell[-1]; - p3=ell[-2]; - p4=ell[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - m21 = p2 * q1; - m31 = p3 * q1; - m41 = p4 * q1; - ell += lskip1; - ex -= 4; - Z11 += m11; - Z21 += m21; - Z31 += m31; - Z41 += m41; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 4; - for (; j > 0; j--) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - p2=ell[-1]; - p3=ell[-2]; - p4=ell[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - m21 = p2 * q1; - m31 = p3 * q1; - m41 = p4 * q1; - ell += lskip1; - ex -= 1; - Z11 += m11; - Z21 += m21; - Z31 += m31; - Z41 += m41; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - p1 = ell[-1]; - Z21 = ex[-1] - Z21 - p1*Z11; - ex[-1] = Z21; - p1 = ell[-2]; - p2 = ell[-2+lskip1]; - Z31 = ex[-2] - Z31 - p1*Z11 - p2*Z21; - ex[-2] = Z31; - p1 = ell[-3]; - p2 = ell[-3+lskip1]; - p3 = ell[-3+lskip2]; - Z41 = ex[-3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31; - ex[-3] = Z41; - /* end of outer loop */ - } - /* compute rows at end that are not a multiple of block size */ - for (; i < n; i++) { - /* compute all 1 x 1 block of X, from rows i..i+1-1 */ - /* set the Z matrix to 0 */ - Z11=0; - ell = L - i; - ex = B; - /* the inner loop that computes outer products and adds them to Z */ - for (j=i-4; j >= 0; j -= 4) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - ell += lskip1; - Z11 += m11; - /* load p and q values */ - p1=ell[0]; - q1=ex[-1]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - ell += lskip1; - Z11 += m11; - /* load p and q values */ - p1=ell[0]; - q1=ex[-2]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - ell += lskip1; - Z11 += m11; - /* load p and q values */ - p1=ell[0]; - q1=ex[-3]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - ell += lskip1; - ex -= 4; - Z11 += m11; - /* end of inner loop */ - } - /* compute left-over iterations */ - j += 4; - for (; j > 0; j--) { - /* load p and q values */ - p1=ell[0]; - q1=ex[0]; - /* compute outer product and add it to the Z matrix */ - m11 = p1 * q1; - ell += lskip1; - ex -= 1; - Z11 += m11; - } - /* finish computing the X(i) block */ - Z11 = ex[0] - Z11; - ex[0] = Z11; - } -} diff --git a/extern/ode/dist/ode/src/geom.cpp b/extern/ode/dist/ode/src/geom.cpp deleted file mode 100644 index 1818814a791..00000000000 --- a/extern/ode/dist/ode/src/geom.cpp +++ /dev/null @@ -1,2207 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -/* - -the rule is that only the low level primitive collision functions should set -dContactGeom::g1 and dContactGeom::g2. - -*/ - -#define SHARED_GEOM_H_INCLUDED_FROM_DEFINING_FILE 1 -#include <ode/common.h> -#include <ode/geom.h> -#include <ode/rotation.h> -#include <ode/odemath.h> -#include <ode/memory.h> -#include <ode/misc.h> -#include <ode/objects.h> -#include <ode/matrix.h> -#include "objects.h" -#include "array.h" -#include "geom_internal.h" - -//**************************************************************************** -// collision utilities. - -// given a pointer `p' to a dContactGeom, return the dContactGeom at -// p + skip bytes. - -#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip))) - - -// if the spheres (p1,r1) and (p2,r2) collide, set the contact `c' and -// return 1, else return 0. - -static int dCollideSpheres (dVector3 p1, dReal r1, - dVector3 p2, dReal r2, dContactGeom *c) -{ - // printf ("d=%.2f (%.2f %.2f %.2f) (%.2f %.2f %.2f) r1=%.2f r2=%.2f\n", - // d,p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],r1,r2); - - dReal d = dDISTANCE (p1,p2); - if (d > (r1 + r2)) return 0; - if (d <= 0) { - c->pos[0] = p1[0]; - c->pos[1] = p1[1]; - c->pos[2] = p1[2]; - c->normal[0] = 1; - c->normal[1] = 0; - c->normal[2] = 0; - c->depth = r1 + r2; - } - else { - dReal d1 = dRecip (d); - c->normal[0] = (p1[0]-p2[0])*d1; - c->normal[1] = (p1[1]-p2[1])*d1; - c->normal[2] = (p1[2]-p2[2])*d1; - dReal k = REAL(0.5) * (r2 - r1 - d); - c->pos[0] = p1[0] + c->normal[0]*k; - c->pos[1] = p1[1] + c->normal[1]*k; - c->pos[2] = p1[2] + c->normal[2]*k; - c->depth = r1 + r2 - d; - } - return 1; -} - - -// given two lines -// qa = pa + alpha* ua -// qb = pb + beta * ub -// where pa,pb are two points, ua,ub are two unit length vectors, and alpha, -// beta go from [-inf,inf], return alpha and beta such that qa and qb are -// as close as possible - -static void lineClosestApproach (const dVector3 pa, const dVector3 ua, - const dVector3 pb, const dVector3 ub, - dReal *alpha, dReal *beta) -{ - dVector3 p; - p[0] = pb[0] - pa[0]; - p[1] = pb[1] - pa[1]; - p[2] = pb[2] - pa[2]; - dReal uaub = dDOT(ua,ub); - dReal q1 = dDOT(ua,p); - dReal q2 = -dDOT(ub,p); - dReal d = 1-uaub*uaub; - if (d <= 0) { - // @@@ this needs to be made more robust - *alpha = 0; - *beta = 0; - } - else { - d = dRecip(d); - *alpha = (q1 + uaub*q2)*d; - *beta = (uaub*q1 + q2)*d; - } -} - - -// given two line segments A and B with endpoints a1-a2 and b1-b2, return the -// points on A and B that are closest to each other (in cp1 and cp2). -// in the case of parallel lines where there are multiple solutions, a -// solution involving the endpoint of at least one line will be returned. -// this will work correctly for zero length lines, e.g. if a1==a2 and/or -// b1==b2. -// -// the algorithm works by applying the voronoi clipping rule to the features -// of the line segments. the three features of each line segment are the two -// endpoints and the line between them. the voronoi clipping rule states that, -// for feature X on line A and feature Y on line B, the closest points PA and -// PB between X and Y are globally the closest points if PA is in V(Y) and -// PB is in V(X), where V(X) is the voronoi region of X. - - -void dClosestLineSegmentPoints (dVector3 const a1, dVector3 const a2, - dVector3 const b1, dVector3 const b2, - dVector3 cp1, dVector3 cp2) -{ - dVector3 a1a2,b1b2,a1b1,a1b2,a2b1,a2b2,n; - dReal la,lb,k,da1,da2,da3,da4,db1,db2,db3,db4,det; - -#define SET2(a,b) a[0]=b[0]; a[1]=b[1]; a[2]=b[2]; -#define SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2]; - - // check vertex-vertex features - - SET3 (a1a2,a2,-,a1); - SET3 (b1b2,b2,-,b1); - SET3 (a1b1,b1,-,a1); - da1 = dDOT(a1a2,a1b1); - db1 = dDOT(b1b2,a1b1); - if (da1 <= 0 && db1 >= 0) { - SET2 (cp1,a1); - SET2 (cp2,b1); - return; - } - - SET3 (a1b2,b2,-,a1); - da2 = dDOT(a1a2,a1b2); - db2 = dDOT(b1b2,a1b2); - if (da2 <= 0 && db2 <= 0) { - SET2 (cp1,a1); - SET2 (cp2,b2); - return; - } - - SET3 (a2b1,b1,-,a2); - da3 = dDOT(a1a2,a2b1); - db3 = dDOT(b1b2,a2b1); - if (da3 >= 0 && db3 >= 0) { - SET2 (cp1,a2); - SET2 (cp2,b1); - return; - } - - SET3 (a2b2,b2,-,a2); - da4 = dDOT(a1a2,a2b2); - db4 = dDOT(b1b2,a2b2); - if (da4 >= 0 && db4 <= 0) { - SET2 (cp1,a2); - SET2 (cp2,b2); - return; - } - - // check edge-vertex features. - // if one or both of the lines has zero length, we will never get to here, - // so we do not have to worry about the following divisions by zero. - - la = dDOT(a1a2,a1a2); - if (da1 >= 0 && da3 <= 0) { - k = da1 / la; - SET3 (n,a1b1,-,k*a1a2); - if (dDOT(b1b2,n) >= 0) { - SET3 (cp1,a1,+,k*a1a2); - SET2 (cp2,b1); - return; - } - } - - if (da2 >= 0 && da4 <= 0) { - k = da2 / la; - SET3 (n,a1b2,-,k*a1a2); - if (dDOT(b1b2,n) <= 0) { - SET3 (cp1,a1,+,k*a1a2); - SET2 (cp2,b2); - return; - } - } - - lb = dDOT(b1b2,b1b2); - if (db1 <= 0 && db2 >= 0) { - k = -db1 / lb; - SET3 (n,-a1b1,-,k*b1b2); - if (dDOT(a1a2,n) >= 0) { - SET2 (cp1,a1); - SET3 (cp2,b1,+,k*b1b2); - return; - } - } - - if (db3 <= 0 && db4 >= 0) { - k = -db3 / lb; - SET3 (n,-a2b1,-,k*b1b2); - if (dDOT(a1a2,n) <= 0) { - SET2 (cp1,a2); - SET3 (cp2,b1,+,k*b1b2); - return; - } - } - - // it must be edge-edge - - k = dDOT(a1a2,b1b2); - det = la*lb - k*k; - if (det <= 0) { - // this should never happen, but just in case... - SET2(cp1,a1); - SET2(cp2,b1); - return; - } - det = dRecip (det); - dReal alpha = (lb*da1 - k*db1) * det; - dReal beta = ( k*da1 - la*db1) * det; - SET3 (cp1,a1,+,alpha*a1a2); - SET3 (cp2,b1,+,beta*b1b2); - -# undef SET2 -# undef SET3 -} - - -// given a line segment p1-p2 and a box (center 'c', rotation 'R', side length -// vector 'side'), compute the points of closest approach between the box -// and the line. return these points in 'lret' (the point on the line) and -// 'bret' (the point on the box). if the line actually penetrates the box -// then the solution is not unique, but only one solution will be returned. -// in this case the solution points will coincide. -// -// a simple root finding algorithm is used to find the value of 't' that -// satisfies: -// d|D(t)|^2/dt = 0 -// where: -// |D(t)| = |p(t)-b(t)| -// where p(t) is a point on the line parameterized by t: -// p(t) = p1 + t*(p2-p1) -// and b(t) is that same point clipped to the boundary of the box. in box- -// relative coordinates d|D(t)|^2/dt is the sum of three x,y,z components -// each of which looks like this: -// -// t_lo / -// ______/ -->t -// / t_hi -// / -// -// t_lo and t_hi are the t values where the line passes through the planes -// corresponding to the sides of the box. the algorithm computes d|D(t)|^2/dt -// in a piecewise fashion from t=0 to t=1, stopping at the point where -// d|D(t)|^2/dt crosses from negative to positive. - -static void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2, - const dVector3 c, const dMatrix3 R, - const dVector3 side, - dVector3 lret, dVector3 bret) -{ - int i; - - // compute the start and delta of the line p1-p2 relative to the box. - // we will do all subsequent computations in this box-relative coordinate - // system. we have to do a translation and rotation for each point. - dVector3 tmp,s,v; - tmp[0] = p1[0] - c[0]; - tmp[1] = p1[1] - c[1]; - tmp[2] = p1[2] - c[2]; - dMULTIPLY1_331 (s,R,tmp); - tmp[0] = p2[0] - p1[0]; - tmp[1] = p2[1] - p1[1]; - tmp[2] = p2[2] - p1[2]; - dMULTIPLY1_331 (v,R,tmp); - - // mirror the line so that v has all components >= 0 - dVector3 sign; - for (i=0; i<3; i++) { - if (v[i] < 0) { - s[i] = -s[i]; - v[i] = -v[i]; - sign[i] = -1; - } - else sign[i] = 1; - } - - // compute v^2 - dVector3 v2; - v2[0] = v[0]*v[0]; - v2[1] = v[1]*v[1]; - v2[2] = v[2]*v[2]; - - // compute the half-sides of the box - dReal h[3]; - h[0] = REAL(0.5) * side[0]; - h[1] = REAL(0.5) * side[1]; - h[2] = REAL(0.5) * side[2]; - - // region is -1,0,+1 depending on which side of the box planes each - // coordinate is on. tanchor in the next t value at which there is a - // transition, or the last one if there are no more. - int region[3]; - dReal tanchor[3]; - - // find the region and tanchor values for p1 - for (i=0; i<3; i++) { - if (v[i] > 0) { - if (s[i] < -h[i]) { - region[i] = -1; - tanchor[i] = (-h[i]-s[i])/v[i]; - } - else { - region[i] = (s[i] > h[i]); - tanchor[i] = (h[i]-s[i])/v[i]; - } - } - else { - region[i] = 0; - tanchor[i] = 2; // this will never be a valid tanchor - } - } - - // compute d|d|^2/dt for t=0. if it's >= 0 then p1 is the closest point - dReal t=0; - dReal dd2dt = 0; - for (i=0; i<3; i++) dd2dt -= (region[i] ? v2[i] : 0) * tanchor[i]; - if (dd2dt >= 0) goto got_answer; - - do { - // find the point on the line that is at the next clip plane boundary - dReal next_t = 1; - for (i=0; i<3; i++) { - if (tanchor[i] > t && tanchor[i] < 1 && tanchor[i] < next_t) - next_t = tanchor[i]; - } - - // compute d|d|^2/dt for the next t - dReal next_dd2dt = 0; - for (i=0; i<3; i++) { - next_dd2dt += (region[i] ? v2[i] : 0) * (next_t - tanchor[i]); - } - - // if the sign of d|d|^2/dt has changed, solution = the crossover point - if (next_dd2dt >= 0) { - dReal m = (next_dd2dt-dd2dt)/(next_t - t); - t -= dd2dt/m; - goto got_answer; - } - - // advance to the next anchor point / region - for (i=0; i<3; i++) { - if (tanchor[i] == next_t) { - tanchor[i] = (h[i]-s[i])/v[i]; - region[i]++; - } - } - t = next_t; - dd2dt = next_dd2dt; - } - while (t < 1); - t = 1; - - got_answer: - - // compute closest point on the line - for (i=0; i<3; i++) lret[i] = p1[i] + t*tmp[i]; // note: tmp=p2-p1 - - // compute closest point on the box - for (i=0; i<3; i++) { - tmp[i] = sign[i] * (s[i] + t*v[i]); - if (tmp[i] < -h[i]) tmp[i] = -h[i]; - else if (tmp[i] > h[i]) tmp[i] = h[i]; - } - dMULTIPLY0_331 (s,R,tmp); - for (i=0; i<3; i++) bret[i] = s[i] + c[i]; -} - - -// given a box (R,side), `R' is the rotation matrix for the box, and `side' -// is a vector of x/y/z side lengths, return the size of the interval of the -// box projected along the given axis. if the axis has unit length then the -// return value will be the actual diameter, otherwise the result will be -// scaled by the axis length. - -static inline dReal boxDiameter (const dMatrix3 R, const dVector3 side, - const dVector3 axis) -{ - dVector3 q; - dMULTIPLY1_331 (q,R,axis); // transform axis to body-relative - return dFabs(q[0])*side[0] + dFabs(q[1])*side[1] + dFabs(q[2])*side[2]; -} - - -// given boxes (p1,R1,side1) and (p1,R1,side1), return 1 if they intersect -// or 0 if not. - -int dBoxTouchesBox (const dVector3 p1, const dMatrix3 R1, - const dVector3 side1, const dVector3 p2, - const dMatrix3 R2, const dVector3 side2) -{ - // two boxes are disjoint if (and only if) there is a separating axis - // perpendicular to a face from one box or perpendicular to an edge from - // either box. the following tests are derived from: - // "OBB Tree: A Hierarchical Structure for Rapid Interference Detection", - // S.Gottschalk, M.C.Lin, D.Manocha., Proc of ACM Siggraph 1996. - - // Rij is R1'*R2, i.e. the relative rotation between R1 and R2. - // Qij is abs(Rij) - dVector3 p,pp; - dReal A1,A2,A3,B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33, - Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33; - - // get vector from centers of box 1 to box 2, relative to box 1 - p[0] = p2[0] - p1[0]; - p[1] = p2[1] - p1[1]; - p[2] = p2[2] - p1[2]; - dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 - - // get side lengths / 2 - A1 = side1[0]*REAL(0.5); A2 = side1[1]*REAL(0.5); A3 = side1[2]*REAL(0.5); - B1 = side2[0]*REAL(0.5); B2 = side2[1]*REAL(0.5); B3 = side2[2]*REAL(0.5); - - // for the following tests, excluding computation of Rij, in the worst case, - // 15 compares, 60 adds, 81 multiplies, and 24 absolutes. - // notation: R1=[u1 u2 u3], R2=[v1 v2 v3] - - // separating axis = u1,u2,u3 - R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); - Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13); - if (dFabs(pp[0]) > (A1 + B1*Q11 + B2*Q12 + B3*Q13)) return 0; - R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); - Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23); - if (dFabs(pp[1]) > (A2 + B1*Q21 + B2*Q22 + B3*Q23)) return 0; - R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); - Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33); - if (dFabs(pp[2]) > (A3 + B1*Q31 + B2*Q32 + B3*Q33)) return 0; - - // separating axis = v1,v2,v3 - if (dFabs(dDOT41(R2+0,p)) > (A1*Q11 + A2*Q21 + A3*Q31 + B1)) return 0; - if (dFabs(dDOT41(R2+1,p)) > (A1*Q12 + A2*Q22 + A3*Q32 + B2)) return 0; - if (dFabs(dDOT41(R2+2,p)) > (A1*Q13 + A2*Q23 + A3*Q33 + B3)) return 0; - - // separating axis = u1 x (v1,v2,v3) - if (dFabs(pp[2]*R21-pp[1]*R31) > A2*Q31 + A3*Q21 + B2*Q13 + B3*Q12) return 0; - if (dFabs(pp[2]*R22-pp[1]*R32) > A2*Q32 + A3*Q22 + B1*Q13 + B3*Q11) return 0; - if (dFabs(pp[2]*R23-pp[1]*R33) > A2*Q33 + A3*Q23 + B1*Q12 + B2*Q11) return 0; - - // separating axis = u2 x (v1,v2,v3) - if (dFabs(pp[0]*R31-pp[2]*R11) > A1*Q31 + A3*Q11 + B2*Q23 + B3*Q22) return 0; - if (dFabs(pp[0]*R32-pp[2]*R12) > A1*Q32 + A3*Q12 + B1*Q23 + B3*Q21) return 0; - if (dFabs(pp[0]*R33-pp[2]*R13) > A1*Q33 + A3*Q13 + B1*Q22 + B2*Q21) return 0; - - // separating axis = u3 x (v1,v2,v3) - if (dFabs(pp[1]*R11-pp[0]*R21) > A1*Q21 + A2*Q11 + B2*Q33 + B3*Q32) return 0; - if (dFabs(pp[1]*R12-pp[0]*R22) > A1*Q22 + A2*Q12 + B1*Q33 + B3*Q31) return 0; - if (dFabs(pp[1]*R13-pp[0]*R23) > A1*Q23 + A2*Q13 + B1*Q32 + B2*Q31) return 0; - - return 1; -} - - -// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and -// generate contact points. this returns 0 if there is no contact otherwise -// it returns the number of contacts generated. -// `normal' returns the contact normal. -// `depth' returns the maximum penetration depth along that normal. -// `code' returns a number indicating the type of contact that was detected: -// 1,2,3 = box 2 intersects with a face of box 1 -// 4,5,6 = box 1 intersects with a face of box 2 -// 7..15 = edge-edge contact -// `maxc' is the maximum number of contacts allowed to be generated, i.e. -// the size of the `contact' array. -// `contact' and `skip' are the contact array information provided to the -// collision functions. this function only fills in the position and depth -// fields. -// -// @@@ some stuff to optimize here, reuse code in contact point calculations. - -extern "C" int dBoxBox (const dVector3 p1, const dMatrix3 R1, - const dVector3 side1, const dVector3 p2, - const dMatrix3 R2, const dVector3 side2, - dVector3 normal, dReal *depth, int *code, - int maxc, dContactGeom *contact, int skip) -{ - dVector3 p,pp,normalC; - const dReal *normalR = 0; - dReal A1,A2,A3,B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33, - Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l; - int i,invert_normal; - - // get vector from centers of box 1 to box 2, relative to box 1 - p[0] = p2[0] - p1[0]; - p[1] = p2[1] - p1[1]; - p[2] = p2[2] - p1[2]; - dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 - - // get side lengths / 2 - A1 = side1[0]*REAL(0.5); A2 = side1[1]*REAL(0.5); A3 = side1[2]*REAL(0.5); - B1 = side2[0]*REAL(0.5); B2 = side2[1]*REAL(0.5); B3 = side2[2]*REAL(0.5); - - // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 - R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); - R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); - R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); - - Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13); - Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23); - Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33); - - // for all 15 possible separating axes: - // * see if the axis separates the boxes. if so, return 0. - // * find the depth of the penetration along the separating axis (s2) - // * if this is the largest depth so far, record it. - // the normal vector will be set to the separating axis with the smallest - // depth. note: normalR is set to point to a column of R1 or R2 if that is - // the smallest depth normal so far. otherwise normalR is 0 and normalC is - // set to a vector relative to body 1. invert_normal is 1 if the sign of - // the normal should be flipped. - -#define TEST(expr1,expr2,norm,cc) \ - s2 = dFabs(expr1) - (expr2); \ - if (s2 > 0) return 0; \ - if (s2 > s) { \ - s = s2; \ - normalR = norm; \ - invert_normal = ((expr1) < 0); \ - *code = (cc); \ - } - - s = -dInfinity; - invert_normal = 0; - *code = 0; - - // separating axis = u1,u2,u3 - TEST (pp[0],(A1 + B1*Q11 + B2*Q12 + B3*Q13),R1+0,1); - TEST (pp[1],(A2 + B1*Q21 + B2*Q22 + B3*Q23),R1+1,2); - TEST (pp[2],(A3 + B1*Q31 + B2*Q32 + B3*Q33),R1+2,3); - - // separating axis = v1,v2,v3 - TEST (dDOT41(R2+0,p),(A1*Q11 + A2*Q21 + A3*Q31 + B1),R2+0,4); - TEST (dDOT41(R2+1,p),(A1*Q12 + A2*Q22 + A3*Q32 + B2),R2+1,5); - TEST (dDOT41(R2+2,p),(A1*Q13 + A2*Q23 + A3*Q33 + B3),R2+2,6); - - // note: cross product axes need to be scaled when s is computed. - // normal (n1,n2,n3) is relative to box 1. -#undef TEST -#define TEST(expr1,expr2,n1,n2,n3,cc) \ - s2 = dFabs(expr1) - (expr2); \ - if (s2 > 0) return 0; \ - l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ - if (l > 0) { \ - s2 /= l; \ - if (s2 > s) { \ - s = s2; \ - normalR = 0; \ - normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \ - invert_normal = ((expr1) < 0); \ - *code = (cc); \ - } \ - } - - // separating axis = u1 x (v1,v2,v3) - TEST(pp[2]*R21-pp[1]*R31,(A2*Q31+A3*Q21+B2*Q13+B3*Q12),0,-R31,R21,7); - TEST(pp[2]*R22-pp[1]*R32,(A2*Q32+A3*Q22+B1*Q13+B3*Q11),0,-R32,R22,8); - TEST(pp[2]*R23-pp[1]*R33,(A2*Q33+A3*Q23+B1*Q12+B2*Q11),0,-R33,R23,9); - - // separating axis = u2 x (v1,v2,v3) - TEST(pp[0]*R31-pp[2]*R11,(A1*Q31+A3*Q11+B2*Q23+B3*Q22),R31,0,-R11,10); - TEST(pp[0]*R32-pp[2]*R12,(A1*Q32+A3*Q12+B1*Q23+B3*Q21),R32,0,-R12,11); - TEST(pp[0]*R33-pp[2]*R13,(A1*Q33+A3*Q13+B1*Q22+B2*Q21),R33,0,-R13,12); - - // separating axis = u3 x (v1,v2,v3) - TEST(pp[1]*R11-pp[0]*R21,(A1*Q21+A2*Q11+B2*Q33+B3*Q32),-R21,R11,0,13); - TEST(pp[1]*R12-pp[0]*R22,(A1*Q22+A2*Q12+B1*Q33+B3*Q31),-R22,R12,0,14); - TEST(pp[1]*R13-pp[0]*R23,(A1*Q23+A2*Q13+B1*Q32+B2*Q31),-R23,R13,0,15); - -#undef TEST - - // if we get to this point, the boxes interpenetrate. compute the normal - // in global coordinates. - if (normalR) { - normal[0] = normalR[0]; - normal[1] = normalR[4]; - normal[2] = normalR[8]; - } - else { - dMULTIPLY0_331 (normal,R1,normalC); - } - if (invert_normal) { - normal[0] = -normal[0]; - normal[1] = -normal[1]; - normal[2] = -normal[2]; - } - *depth = -s; - - // compute contact point(s) - - if (*code > 6) { - // an edge from box 1 touches an edge from box 2. - // find a point pa on the intersecting edge of box 1 - dVector3 pa; - dReal sign; - for (i=0; i<3; i++) pa[i] = p1[i]; - sign = (dDOT14(normal,R1+0) > 0) ? REAL(1.0) : REAL(-1.0); - for (i=0; i<3; i++) pa[i] += sign * A1 * R1[i*4]; - sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0); - for (i=0; i<3; i++) pa[i] += sign * A2 * R1[i*4+1]; - sign = (dDOT14(normal,R1+2) > 0) ? REAL(1.0) : REAL(-1.0); - for (i=0; i<3; i++) pa[i] += sign * A3 * R1[i*4+2]; - - // find a point pb on the intersecting edge of box 2 - dVector3 pb; - for (i=0; i<3; i++) pb[i] = p2[i]; - sign = (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0); - for (i=0; i<3; i++) pb[i] += sign * B1 * R2[i*4]; - sign = (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0); - for (i=0; i<3; i++) pb[i] += sign * B2 * R2[i*4+1]; - sign = (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0); - for (i=0; i<3; i++) pb[i] += sign * B3 * R2[i*4+2]; - - dReal alpha,beta; - dVector3 ua,ub; - for (i=0; i<3; i++) ua[i] = R1[((*code)-7)/3 + i*4]; - for (i=0; i<3; i++) ub[i] = R2[((*code)-7)%3 + i*4]; - - lineClosestApproach (pa,ua,pb,ub,&alpha,&beta); - for (i=0; i<3; i++) pa[i] += ua[i]*alpha; - for (i=0; i<3; i++) pb[i] += ub[i]*beta; - - for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]); - contact[0].depth = *depth; - return 1; - } - - // okay, we have a face-something intersection (because the separating - // axis is perpendicular to a face). - - // @@@ temporary: make deepest vertex on the "other" box the contact point. - // @@@ this kind of works, but we need multiple contact points for stability, - // @@@ especially for face-face contact. - - dVector3 vertex; - if (*code <= 3) { - // face from box 1 touches a vertex/edge/face from box 2. - dReal sign; - for (i=0; i<3; i++) vertex[i] = p2[i]; - sign = (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0); - for (i=0; i<3; i++) vertex[i] += sign * B1 * R2[i*4]; - sign = (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0); - for (i=0; i<3; i++) vertex[i] += sign * B2 * R2[i*4+1]; - sign = (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0); - for (i=0; i<3; i++) vertex[i] += sign * B3 * R2[i*4+2]; - } - else { - // face from box 2 touches a vertex/edge/face from box 1. - dReal sign; - for (i=0; i<3; i++) vertex[i] = p1[i]; - sign = (dDOT14(normal,R1+0) > 0) ? REAL(1.0) : REAL(-1.0); - for (i=0; i<3; i++) vertex[i] += sign * A1 * R1[i*4]; - sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0); - for (i=0; i<3; i++) vertex[i] += sign * A2 * R1[i*4+1]; - sign = (dDOT14(normal,R1+2) > 0) ? REAL(1.0) : REAL(-1.0); - for (i=0; i<3; i++) vertex[i] += sign * A3 * R1[i*4+2]; - } - for (i=0; i<3; i++) contact[0].pos[i] = vertex[i]; - contact[0].depth = *depth; - return 1; -} - -//**************************************************************************** -// general support for geometry objects and classes - -struct dColliderEntry { - dColliderFn *fn; // collider function - int mode; // 1 = reverse o1 and o2, 2 = no function available -}; - -static dArray<dxGeomClass*> *classes=0; - -// function pointers and modes for n^2 class collider functions. this is an -// n*n matrix stored by row. the functions pointers are extracted from the -// class get-collider-function function. -static dArray<dColliderEntry> *colliders=0; - - -static inline void initCollisionArrays() -{ - if (classes==0) { - // old way: - // classes = (dArray<dxGeomClass*> *) dAllocNoFree (sizeof(dArrayBase)); - // classes->constructor(); - classes = new dArray<dxGeomClass*>; - classes->setSize (1); // force allocation of array data memory - dAllocDontReport (classes); - dAllocDontReport (classes->data()); - classes->setSize (0); - } - if (colliders==0) { - // old way: - // colliders=(dArray<dColliderEntry> *)dAllocNoFree (sizeof(dArrayBase)); - // colliders->constructor(); - colliders = new dArray<dColliderEntry>; - colliders->setSize (1); // force allocation of array data memory - dAllocDontReport (colliders); - dAllocDontReport (colliders->data()); - colliders->setSize (0); - } -} - - -int dCreateGeomClass (const dGeomClass *c) -{ - dUASSERT(c && c->bytes >= 0 && c->collider && c->aabb,"bad geom class"); - initCollisionArrays(); - - int n = classes->size(); - dxGeomClass *gc = (dxGeomClass*) dAlloc (sizeof(dxGeomClass)); - dAllocDontReport (gc); - gc->collider = c->collider; - gc->aabb = c->aabb; - gc->aabb_test = c->aabb_test; - gc->dtor = c->dtor; - gc->num = n; - gc->size = SIZEOF_DXGEOM + c->bytes; - classes->push (gc); - - // make room for n^2 class collider function pointers - these entries will - // be filled as dCollide() is called. - colliders->setSize ((n+1)*(n+1)); - memset (colliders->data(),0,(n+1)*(n+1)*sizeof(dColliderEntry)); - - return n; -} - - -int dCollide (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, - int skip) -{ - int i,c1,c2,a1,a2,count,swap; - dColliderFn *fn; - dAASSERT(o1 && o2 && contact); - dUASSERT(classes && colliders,"no registered geometry classes"); - - // no contacts if both geoms on the same body, and the body is not 0 - if (o1->body == o2->body && o1->body) return 0; - - dColliderEntry *colliders2 = colliders->data(); - c1 = o1->_class->num; - c2 = o2->_class->num; - a1 = c1 * classes->size() + c2; // address 1 in collider array - a2 = c2 * classes->size() + c1; // address 2 in collider array - swap = 0; // set to 1 to swap normals before returning - - // return if there are no collider functions available - if ((colliders2[a1].mode==2) || (colliders2[a2].mode==2)) return 0; - - if ((fn = colliders2[a1].fn)) { - swap = colliders2[a1].mode; - if (swap) count = (*fn) (o2,o1,flags,contact,skip); - else count = (*fn) (o1,o2,flags,contact,skip); - } - else if ((fn = (*classes)[c1]->collider (c2))) { - colliders2 [a2].fn = fn; - colliders2 [a2].mode = 1; - colliders2 [a1].fn = fn; // do mode=0 assignment second so that - colliders2 [a1].mode = 0; // diagonal entries will have mode 0 - count = (*fn) (o1,o2,flags,contact,skip); - swap = 0; - } - else if ((fn = (*classes)[c2]->collider (c1))) { - colliders2 [a1].fn = fn; - colliders2 [a1].mode = 1; - colliders2 [a2].fn = fn; // do mode=0 assignment second so that - colliders2 [a2].mode = 0; // diagonal entries will have mode 0 - count = (*fn) (o2,o1,flags,contact,skip); - swap = 1; - } - else { - colliders2[a1].mode = 2; - colliders2[a2].mode = 2; - return 0; - } - - if (swap) { - for (i=0; i<count; i++) { - dContactGeom *c = CONTACT(contact,skip*i); - c->normal[0] = -c->normal[0]; - c->normal[1] = -c->normal[1]; - c->normal[2] = -c->normal[2]; - dxGeom *tmp = c->g1; - c->g1 = c->g2; - c->g2 = tmp; - } - } - - return count; -} - - -int dGeomGetClass (dxGeom *g) -{ - dAASSERT (g); - return g->_class->num; -} - - -void dGeomSetData (dxGeom *g, void *data) -{ - dAASSERT (g); - g->data = data; -} - - -void *dGeomGetData (dxGeom *g) -{ - dAASSERT (g); - return g->data; -} - - -void dGeomSetBody (dxGeom *g, dBodyID b) -{ - dAASSERT (g); - if (b) { - if (!g->body) dFree (g->pos,sizeof(dxPosR)); - g->body = b; - g->pos = b->pos; - g->R = b->R; - } - else { - if (g->body) { - dxPosR *pr = (dxPosR*) dAlloc (sizeof(dxPosR)); - g->pos = pr->pos; - g->R = pr->R; - memcpy (g->pos,g->body->pos,sizeof(g->pos)); - memcpy (g->R,g->body->R,sizeof(g->R)); - g->body = 0; - } - } -} - - -dBodyID dGeomGetBody (dxGeom *g) -{ - dAASSERT (g); - return g->body; -} - - -void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z) -{ - dAASSERT (g); - if (g->body) dBodySetPosition (g->body,x,y,z); - else { - g->pos[0] = x; - g->pos[1] = y; - g->pos[2] = z; - } -} - - -void dGeomSetRotation (dxGeom *g, const dMatrix3 R) -{ - dAASSERT (g); - if (g->body) dBodySetRotation (g->body,R); - else memcpy (g->R,R,sizeof(dMatrix3)); -} - - -const dReal * dGeomGetPosition (dxGeom *g) -{ - dAASSERT (g); - return g->pos; -} - - -const dReal * dGeomGetRotation (dxGeom *g) -{ - dAASSERT (g); - return g->R; -} - - -// for external use only. use the CLASSDATA macro inside ODE. - -void * dGeomGetClassData (dxGeom *g) -{ - dAASSERT (g); - return (void*) CLASSDATA(g); -} - - -dxGeom * dCreateGeom (int classnum) -{ - dUASSERT (classes && colliders && classnum >= 0 && - classnum < classes->size(),"bad class number"); - int size = (*classes)[classnum]->size; - dxGeom *geom = (dxGeom*) dAlloc (size); - memset (geom,0,size); // everything is initially zeroed - - geom->_class = (*classes)[classnum]; - geom->data = 0; - geom->body = 0; - - dxPosR *pr = (dxPosR*) dAlloc (sizeof(dxPosR)); - geom->pos = pr->pos; - geom->R = pr->R; - dSetZero (geom->pos,4); - dRSetIdentity (geom->R); - - return geom; -} - - -void dGeomDestroy (dxGeom *g) -{ - dAASSERT (g); - if (g->spaceid) dSpaceRemove (g->spaceid,g); - if (g->_class->dtor) g->_class->dtor (g); - if (!g->body) dFree (g->pos,sizeof(dxPosR)); - dFree (g,g->_class->size); -} - - -void dGeomGetAABB (dxGeom *g, dReal aabb[6]) -{ - dAASSERT (g); - g->_class->aabb (g,aabb); -} - - -dReal *dGeomGetSpaceAABB (dxGeom *g) -{ - dAASSERT (g); - return g->space_aabb; -} - -//**************************************************************************** -// data for the standard classes - -struct dxSphere { - dReal radius; // sphere radius -}; - -struct dxBox { - dVector3 side; // side lengths (x,y,z) -}; - -struct dxCCylinder { // capped cylinder - dReal radius,lz; // radius, length along z axis */ -}; - -struct dxPlane { - dReal p[4]; -}; - -struct dxGeomGroup { - dArray<dxGeom*> parts; // all the geoms that make up the group -}; - -//**************************************************************************** -// primitive collision functions -// same interface as dCollide(). -// S=sphere, B=box, C=capped cylinder, P=plane, G=group, T=transform - -int dCollideSS (const dxGeom *o1, const dxGeom *o2, int flags, - dContactGeom *contact, int skip) -{ - dIASSERT (skip >= (int)sizeof(dContactGeom)); - dIASSERT (o1->_class->num == dSphereClass); - dIASSERT (o2->_class->num == dSphereClass); - dxSphere *s1 = (dxSphere*) CLASSDATA(o1); - dxSphere *s2 = (dxSphere*) CLASSDATA(o2); - contact->g1 = const_cast<dxGeom*> (o1); - contact->g2 = const_cast<dxGeom*> (o2); - return dCollideSpheres (o1->pos,s1->radius, - o2->pos,s2->radius,contact); -} - - -int dCollideSB (const dxGeom *o1, const dxGeom *o2, int flags, - dContactGeom *contact, int skip) -{ - // this is easy. get the sphere center `p' relative to the box, and then clip - // that to the boundary of the box (call that point `q'). if q is on the - // boundary of the box and |p-q| is <= sphere radius, they touch. - // if q is inside the box, the sphere is inside the box, so set a contact - // normal to push the sphere to the closest box edge. - - dVector3 l,t,p,q,r; - dReal depth; - int onborder = 0; - - dIASSERT (skip >= (int)sizeof(dContactGeom)); - dIASSERT (o1->_class->num == dSphereClass); - dIASSERT (o2->_class->num == dBoxClass); - dxSphere *sphere = (dxSphere*) CLASSDATA(o1); - dxBox *box = (dxBox*) CLASSDATA(o2); - - contact->g1 = const_cast<dxGeom*> (o1); - contact->g2 = const_cast<dxGeom*> (o2); - - p[0] = o1->pos[0] - o2->pos[0]; - p[1] = o1->pos[1] - o2->pos[1]; - p[2] = o1->pos[2] - o2->pos[2]; - - l[0] = box->side[0]*REAL(0.5); - t[0] = dDOT14(p,o2->R); - if (t[0] < -l[0]) { t[0] = -l[0]; onborder = 1; } - if (t[0] > l[0]) { t[0] = l[0]; onborder = 1; } - - l[1] = box->side[1]*REAL(0.5); - t[1] = dDOT14(p,o2->R+1); - if (t[1] < -l[1]) { t[1] = -l[1]; onborder = 1; } - if (t[1] > l[1]) { t[1] = l[1]; onborder = 1; } - - t[2] = dDOT14(p,o2->R+2); - l[2] = box->side[2]*REAL(0.5); - if (t[2] < -l[2]) { t[2] = -l[2]; onborder = 1; } - if (t[2] > l[2]) { t[2] = l[2]; onborder = 1; } - - if (!onborder) { - // sphere center inside box. find largest `t' value - dReal max = dFabs(t[0]); - int maxi = 0; - for (int i=1; i<3; i++) { - dReal tt = dFabs(t[i]); - if (tt > max) { - max = tt; - maxi = i; - } - } - // contact position = sphere center - contact->pos[0] = o1->pos[0]; - contact->pos[1] = o1->pos[1]; - contact->pos[2] = o1->pos[2]; - // contact normal aligned with box edge along largest `t' value - dVector3 tmp; - tmp[0] = 0; - tmp[1] = 0; - tmp[2] = 0; - tmp[maxi] = (t[maxi] > 0) ? REAL(1.0) : REAL(-1.0); - dMULTIPLY0_331 (contact->normal,o2->R,tmp); - // contact depth = distance to wall along normal plus radius - contact->depth = l[maxi] - max + sphere->radius; - return 1; - } - - t[3] = 0; //@@@ hmmm - dMULTIPLY0_331 (q,o2->R,t); - r[0] = p[0] - q[0]; - r[1] = p[1] - q[1]; - r[2] = p[2] - q[2]; - depth = sphere->radius - dSqrt(dDOT(r,r)); - if (depth < 0) return 0; - contact->pos[0] = q[0] + o2->pos[0]; - contact->pos[1] = q[1] + o2->pos[1]; - contact->pos[2] = q[2] + o2->pos[2]; - contact->normal[0] = r[0]; - contact->normal[1] = r[1]; - contact->normal[2] = r[2]; - dNormalize3 (contact->normal); - contact->depth = depth; - return 1; -} - - -int dCollideSP (const dxGeom *o1, const dxGeom *o2, int flags, - dContactGeom *contact, int skip) -{ - dIASSERT (skip >= (int)sizeof(dContactGeom)); - dIASSERT (o1->_class->num == dSphereClass); - dIASSERT (o2->_class->num == dPlaneClass); - contact->g1 = const_cast<dxGeom*> (o1); - contact->g2 = const_cast<dxGeom*> (o2); - dxSphere *sphere = (dxSphere*) CLASSDATA(o1); - dxPlane *plane = (dxPlane*) CLASSDATA(o2); - dReal k = dDOT (o1->pos,plane->p); - dReal depth = plane->p[3] - k + sphere->radius; - if (depth >= 0) { - contact->normal[0] = plane->p[0]; - contact->normal[1] = plane->p[1]; - contact->normal[2] = plane->p[2]; - contact->pos[0] = o1->pos[0] - plane->p[0] * sphere->radius; - contact->pos[1] = o1->pos[1] - plane->p[1] * sphere->radius; - contact->pos[2] = o1->pos[2] - plane->p[2] * sphere->radius; - contact->depth = depth; - return 1; - } - else return 0; -} - - -int dCollideBB (const dxGeom *o1, const dxGeom *o2, int flags, - dContactGeom *contact, int skip) -{ - dVector3 normal; - dReal depth; - int code; - dxBox *b1 = (dxBox*) CLASSDATA(o1); - dxBox *b2 = (dxBox*) CLASSDATA(o2); - int num = dBoxBox (o1->pos,o1->R,b1->side, o2->pos,o2->R,b2->side, - normal,&depth,&code,flags & NUMC_MASK,contact,skip); - for (int i=0; i<num; i++) { - CONTACT(contact,i*skip)->normal[0] = -normal[0]; - CONTACT(contact,i*skip)->normal[1] = -normal[1]; - CONTACT(contact,i*skip)->normal[2] = -normal[2]; - CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1); - CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2); - } - return num; -} - - -int dCollideBP (const dxGeom *o1, const dxGeom *o2, - int flags, dContactGeom *contact, int skip) -{ - dIASSERT (skip >= (int)sizeof(dContactGeom)); - dIASSERT (o1->_class->num == dBoxClass); - dIASSERT (o2->_class->num == dPlaneClass); - contact->g1 = const_cast<dxGeom*> (o1); - contact->g2 = const_cast<dxGeom*> (o2); - dxBox *box = (dxBox*) CLASSDATA(o1); - dxPlane *plane = (dxPlane*) CLASSDATA(o2); - int ret = 0; - - //@@@ problem: using 4-vector (plane->p) as 3-vector (normal). - const dReal *R = o1->R; // rotation of box - const dReal *n = plane->p; // normal vector - - // project sides lengths along normal vector, get absolute values - dReal Q1 = dDOT14(n,R+0); - dReal Q2 = dDOT14(n,R+1); - dReal Q3 = dDOT14(n,R+2); - dReal A1 = box->side[0] * Q1; - dReal A2 = box->side[1] * Q2; - dReal A3 = box->side[2] * Q3; - dReal B1 = dFabs(A1); - dReal B2 = dFabs(A2); - dReal B3 = dFabs(A3); - - // early exit test - dReal depth = plane->p[3] + REAL(0.5)*(B1+B2+B3) - dDOT(n,o1->pos); - if (depth < 0) return 0; - - // find number of contacts requested - int maxc = flags & NUMC_MASK; - if (maxc < 1) maxc = 1; - if (maxc > 3) maxc = 3; // no more than 3 contacts per box allowed - - // find deepest point - dVector3 p; - p[0] = o1->pos[0]; - p[1] = o1->pos[1]; - p[2] = o1->pos[2]; -#define FOO(i,op) \ - p[0] op REAL(0.5)*box->side[i] * R[0+i]; \ - p[1] op REAL(0.5)*box->side[i] * R[4+i]; \ - p[2] op REAL(0.5)*box->side[i] * R[8+i]; -#define BAR(i,iinc) if (A ## iinc > 0) { FOO(i,-=) } else { FOO(i,+=) } - BAR(0,1); - BAR(1,2); - BAR(2,3); -#undef FOO -#undef BAR - - // the deepest point is the first contact point - contact->pos[0] = p[0]; - contact->pos[1] = p[1]; - contact->pos[2] = p[2]; - contact->normal[0] = n[0]; - contact->normal[1] = n[1]; - contact->normal[2] = n[2]; - contact->depth = depth; - ret = 1; // ret is number of contact points found so far - if (maxc == 1) goto done; - - // get the second and third contact points by starting from `p' and going - // along the two sides with the smallest projected length. - -#define FOO(i,j,op) \ - CONTACT(contact,i*skip)->pos[0] = p[0] op box->side[j] * R[0+j]; \ - CONTACT(contact,i*skip)->pos[1] = p[1] op box->side[j] * R[4+j]; \ - CONTACT(contact,i*skip)->pos[2] = p[2] op box->side[j] * R[8+j]; -#define BAR(ctact,side,sideinc) \ - depth -= B ## sideinc; \ - if (depth < 0) goto done; \ - if (A ## sideinc > 0) { FOO(ctact,side,+) } else { FOO(ctact,side,-) } \ - CONTACT(contact,ctact*skip)->depth = depth; \ - ret++; - - CONTACT(contact,skip)->normal[0] = n[0]; - CONTACT(contact,skip)->normal[1] = n[1]; - CONTACT(contact,skip)->normal[2] = n[2]; - if (maxc == 3) { - CONTACT(contact,2*skip)->normal[0] = n[0]; - CONTACT(contact,2*skip)->normal[1] = n[1]; - CONTACT(contact,2*skip)->normal[2] = n[2]; - } - - if (B1 < B2) { - if (B3 < B1) goto use_side_3; else { - BAR(1,0,1); // use side 1 - if (maxc == 2) goto done; - if (B2 < B3) goto contact2_2; else goto contact2_3; - } - } - else { - if (B3 < B2) { - use_side_3: // use side 3 - BAR(1,2,3); - if (maxc == 2) goto done; - if (B1 < B2) goto contact2_1; else goto contact2_2; - } - else { - BAR(1,1,2); // use side 2 - if (maxc == 2) goto done; - if (B1 < B3) goto contact2_1; else goto contact2_3; - } - } - - contact2_1: BAR(2,0,1); goto done; - contact2_2: BAR(2,1,2); goto done; - contact2_3: BAR(2,2,3); goto done; -#undef FOO -#undef BAR - - done: - for (int i=0; i<ret; i++) { - CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1); - CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2); - } - return ret; -} - - -int dCollideCS (const dxGeom *o1, const dxGeom *o2, int flags, - dContactGeom *contact, int skip) -{ - dIASSERT (skip >= (int)sizeof(dContactGeom)); - dIASSERT (o1->_class->num == dCCylinderClass); - dIASSERT (o2->_class->num == dSphereClass); - contact->g1 = const_cast<dxGeom*> (o1); - contact->g2 = const_cast<dxGeom*> (o2); - dxCCylinder *ccyl = (dxCCylinder*) CLASSDATA(o1); - dxSphere *sphere = (dxSphere*) CLASSDATA(o2); - - // find the point on the cylinder axis that is closest to the sphere - dReal alpha = - o1->R[2] * (o2->pos[0] - o1->pos[0]) + - o1->R[6] * (o2->pos[1] - o1->pos[1]) + - o1->R[10] * (o2->pos[2] - o1->pos[2]); - dReal lz2 = ccyl->lz * REAL(0.5); - if (alpha > lz2) alpha = lz2; - if (alpha < -lz2) alpha = -lz2; - - // collide the spheres - dVector3 p; - p[0] = o1->pos[0] + alpha * o1->R[2]; - p[1] = o1->pos[1] + alpha * o1->R[6]; - p[2] = o1->pos[2] + alpha * o1->R[10]; - return dCollideSpheres (p,ccyl->radius,o2->pos,sphere->radius,contact); -} - - -int dCollideCB (const dxGeom *o1, const dxGeom *o2, int flags, - dContactGeom *contact, int skip) -{ - dIASSERT (skip >= (int)sizeof(dContactGeom)); - dIASSERT (o1->_class->num == dCCylinderClass); - dIASSERT (o2->_class->num == dBoxClass); - contact->g1 = const_cast<dxGeom*> (o1); - contact->g2 = const_cast<dxGeom*> (o2); - dxCCylinder *cyl = (dxCCylinder*) CLASSDATA(o1); - dxBox *box = (dxBox*) CLASSDATA(o2); - - // get p1,p2 = cylinder axis endpoints, get radius - dVector3 p1,p2; - dReal clen = cyl->lz * REAL(0.5); - p1[0] = o1->pos[0] + clen * o1->R[2]; - p1[1] = o1->pos[1] + clen * o1->R[6]; - p1[2] = o1->pos[2] + clen * o1->R[10]; - p2[0] = o1->pos[0] - clen * o1->R[2]; - p2[1] = o1->pos[1] - clen * o1->R[6]; - p2[2] = o1->pos[2] - clen * o1->R[10]; - dReal radius = cyl->radius; - - // copy out box center, rotation matrix, and side array - dReal *c = o2->pos; - dReal *R = o2->R; - dReal *side = box->side; - - // get the closest point between the cylinder axis and the box - dVector3 pl,pb; - dClosestLineBoxPoints (p1,p2,c,R,side,pl,pb); - - // generate contact point - return dCollideSpheres (pl,radius,pb,0,contact); -} - - -// this returns at most one contact point when the two cylinder's axes are not -// aligned, and at most two (for stability) when they are aligned. -// the algorithm minimizes the distance between two "sample spheres" that are -// positioned along the cylinder axes according to: -// sphere1 = pos1 + alpha1 * axis1 -// sphere2 = pos2 + alpha2 * axis2 -// alpha1 and alpha2 are limited to +/- half the length of the cylinders. -// the algorithm works by finding a solution that has both alphas free, or -// a solution that has one or both alphas fixed to the ends of the cylinder. - -int dCollideCC (const dxGeom *o1, const dxGeom *o2, - int flags, dContactGeom *contact, int skip) -{ - int i; - const dReal tolerance = REAL(1e-5); - - dIASSERT (skip >= (int)sizeof(dContactGeom)); - dIASSERT (o1->_class->num == dCCylinderClass); - dIASSERT (o2->_class->num == dCCylinderClass); - contact->g1 = const_cast<dxGeom*> (o1); - contact->g2 = const_cast<dxGeom*> (o2); - dxCCylinder *cyl1 = (dxCCylinder*) CLASSDATA(o1); - dxCCylinder *cyl2 = (dxCCylinder*) CLASSDATA(o2); - - // copy out some variables, for convenience - dReal lz1 = cyl1->lz * REAL(0.5); - dReal lz2 = cyl2->lz * REAL(0.5); - dReal *pos1 = o1->pos; - dReal *pos2 = o2->pos; - dReal axis1[3],axis2[3]; - axis1[0] = o1->R[2]; - axis1[1] = o1->R[6]; - axis1[2] = o1->R[10]; - axis2[0] = o2->R[2]; - axis2[1] = o2->R[6]; - axis2[2] = o2->R[10]; - - dReal alpha1,alpha2,sphere1[3],sphere2[3]; - int fix1 = 0; // 0 if alpha1 is free, +/-1 to fix at +/- lz1 - int fix2 = 0; // 0 if alpha2 is free, +/-1 to fix at +/- lz2 - - for (int count=0; count<9; count++) { - // find a trial solution by fixing or not fixing the alphas - if (fix1) { - if (fix2) { - // alpha1 and alpha2 are fixed, so the solution is easy - if (fix1 > 0) alpha1 = lz1; else alpha1 = -lz1; - if (fix2 > 0) alpha2 = lz2; else alpha2 = -lz2; - for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; - for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; - } - else { - // fix alpha1 but let alpha2 be free - if (fix1 > 0) alpha1 = lz1; else alpha1 = -lz1; - for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; - alpha2 = (axis2[0]*(sphere1[0]-pos2[0]) + - axis2[1]*(sphere1[1]-pos2[1]) + - axis2[2]*(sphere1[2]-pos2[2])); - for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; - } - } - else { - if (fix2) { - // fix alpha2 but let alpha1 be free - if (fix2 > 0) alpha2 = lz2; else alpha2 = -lz2; - for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; - alpha1 = (axis1[0]*(sphere2[0]-pos1[0]) + - axis1[1]*(sphere2[1]-pos1[1]) + - axis1[2]*(sphere2[2]-pos1[2])); - for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; - } - else { - // let alpha1 and alpha2 be free - // compute determinant of d(d^2)\d(alpha) jacobian - dReal a1a2 = dDOT (axis1,axis2); - dReal det = REAL(1.0)-a1a2*a1a2; - if (det < tolerance) { - // the cylinder axes (almost) parallel, so we will generate up to two - // contacts. the solution matrix is rank deficient so alpha1 and - // alpha2 are related by: - // alpha2 = alpha1 + (pos1-pos2)'*axis1 (if axis1==axis2) - // or alpha2 = -(alpha1 + (pos1-pos2)'*axis1) (if axis1==-axis2) - // first compute where the two cylinders overlap in alpha1 space: - if (a1a2 < 0) { - axis2[0] = -axis2[0]; - axis2[1] = -axis2[1]; - axis2[2] = -axis2[2]; - } - dReal q[3]; - for (i=0; i<3; i++) q[i] = pos1[i]-pos2[i]; - dReal k = dDOT (axis1,q); - dReal a1lo = -lz1; - dReal a1hi = lz1; - dReal a2lo = -lz2 - k; - dReal a2hi = lz2 - k; - dReal lo = (a1lo > a2lo) ? a1lo : a2lo; - dReal hi = (a1hi < a2hi) ? a1hi : a2hi; - if (lo <= hi) { - int num_contacts = flags & NUMC_MASK; - if (num_contacts >= 2 && lo < hi) { - // generate up to two contacts. if one of those contacts is - // not made, fall back on the one-contact strategy. - for (i=0; i<3; i++) sphere1[i] = pos1[i] + lo*axis1[i]; - for (i=0; i<3; i++) sphere2[i] = pos2[i] + (lo+k)*axis2[i]; - int n1 = dCollideSpheres (sphere1,cyl1->radius, - sphere2,cyl2->radius,contact); - if (n1) { - for (i=0; i<3; i++) sphere1[i] = pos1[i] + hi*axis1[i]; - for (i=0; i<3; i++) sphere2[i] = pos2[i] + (hi+k)*axis2[i]; - dContactGeom *c2 = CONTACT(contact,skip); - int n2 = dCollideSpheres (sphere1,cyl1->radius, - sphere2,cyl2->radius, c2); - if (n2) { - c2->g1 = const_cast<dxGeom*> (o1); - c2->g2 = const_cast<dxGeom*> (o2); - return 2; - } - } - } - - // just one contact to generate, so put it in the middle of - // the range - alpha1 = (lo + hi) * REAL(0.5); - alpha2 = alpha1 + k; - for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; - for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; - return dCollideSpheres (sphere1,cyl1->radius, - sphere2,cyl2->radius,contact); - } - else return 0; - } - det = REAL(1.0)/det; - dReal delta[3]; - for (i=0; i<3; i++) delta[i] = pos1[i] - pos2[i]; - dReal q1 = dDOT (delta,axis1); - dReal q2 = dDOT (delta,axis2); - alpha1 = det*(a1a2*q2-q1); - alpha2 = det*(q2-a1a2*q1); - for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; - for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; - } - } - - // if the alphas are outside their allowed ranges then fix them and - // try again - if (fix1==0) { - if (alpha1 < -lz1) { - fix1 = -1; - continue; - } - if (alpha1 > lz1) { - fix1 = 1; - continue; - } - } - if (fix2==0) { - if (alpha2 < -lz2) { - fix2 = -1; - continue; - } - if (alpha2 > lz2) { - fix2 = 1; - continue; - } - } - - // unfix the alpha variables if the local distance gradient indicates - // that we are not yet at the minimum - dReal tmp[3]; - for (i=0; i<3; i++) tmp[i] = sphere1[i] - sphere2[i]; - if (fix1) { - dReal gradient = dDOT (tmp,axis1); - if ((fix1 > 0 && gradient > 0) || (fix1 < 0 && gradient < 0)) { - fix1 = 0; - continue; - } - } - if (fix2) { - dReal gradient = -dDOT (tmp,axis2); - if ((fix2 > 0 && gradient > 0) || (fix2 < 0 && gradient < 0)) { - fix2 = 0; - continue; - } - } - return dCollideSpheres (sphere1,cyl1->radius,sphere2,cyl2->radius,contact); - } - // if we go through the loop too much, then give up. we should NEVER get to - // this point (i hope). - dMessage (0,"dCollideCC(): too many iterations"); - return 0; -} - - -int dCollideCP (const dxGeom *o1, const dxGeom *o2, int flags, - dContactGeom *contact, int skip) -{ - dIASSERT (skip >= (int)sizeof(dContactGeom)); - dIASSERT (o1->_class->num == dCCylinderClass); - dIASSERT (o2->_class->num == dPlaneClass); - dxCCylinder *ccyl = (dxCCylinder*) CLASSDATA(o1); - dxPlane *plane = (dxPlane*) CLASSDATA(o2); - - // collide the deepest capping sphere with the plane - dReal sign = (dDOT14 (plane->p,o1->R+2) > 0) ? REAL(-1.0) : REAL(1.0); - dVector3 p; - p[0] = o1->pos[0] + o1->R[2] * ccyl->lz * REAL(0.5) * sign; - p[1] = o1->pos[1] + o1->R[6] * ccyl->lz * REAL(0.5) * sign; - p[2] = o1->pos[2] + o1->R[10] * ccyl->lz * REAL(0.5) * sign; - - dReal k = dDOT (p,plane->p); - dReal depth = plane->p[3] - k + ccyl->radius; - if (depth < 0) return 0; - contact->normal[0] = plane->p[0]; - contact->normal[1] = plane->p[1]; - contact->normal[2] = plane->p[2]; - contact->pos[0] = p[0] - plane->p[0] * ccyl->radius; - contact->pos[1] = p[1] - plane->p[1] * ccyl->radius; - contact->pos[2] = p[2] - plane->p[2] * ccyl->radius; - contact->depth = depth; - - int ncontacts = 1; - if ((flags & NUMC_MASK) >= 2) { - // collide the other capping sphere with the plane - p[0] = o1->pos[0] - o1->R[2] * ccyl->lz * REAL(0.5) * sign; - p[1] = o1->pos[1] - o1->R[6] * ccyl->lz * REAL(0.5) * sign; - p[2] = o1->pos[2] - o1->R[10] * ccyl->lz * REAL(0.5) * sign; - - k = dDOT (p,plane->p); - depth = plane->p[3] - k + ccyl->radius; - if (depth >= 0) { - dContactGeom *c2 = CONTACT(contact,skip); - c2->normal[0] = plane->p[0]; - c2->normal[1] = plane->p[1]; - c2->normal[2] = plane->p[2]; - c2->pos[0] = p[0] - plane->p[0] * ccyl->radius; - c2->pos[1] = p[1] - plane->p[1] * ccyl->radius; - c2->pos[2] = p[2] - plane->p[2] * ccyl->radius; - c2->depth = depth; - ncontacts = 2; - } - } - - for (int i=0; i < ncontacts; i++) { - CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1); - CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2); - } - return ncontacts; -} - - -// this collides a group with another geom. the other geom can also be a -// group, but this case is not handled specially. - -int dCollideG (const dxGeom *o1, const dxGeom *o2, int flags, - dContactGeom *contact, int skip) -{ - dxGeomGroup *gr = (dxGeomGroup*) CLASSDATA(o1); - int numleft = flags & NUMC_MASK; - if (numleft == 0) numleft = 1; - flags &= ~NUMC_MASK; - int num=0,i=0; - while (i < gr->parts.size() && numleft > 0) { - int n = dCollide (gr->parts[i],const_cast<dxGeom*>(o2), - flags | numleft,contact,skip); - contact = CONTACT (contact,skip*n); - numleft -= n; - num += n; - i++; - } - return num; -} - -//**************************************************************************** -// standard classes - -SHAREDLIBEXPORT int dSphereClass = -1; -SHAREDLIBEXPORT int dBoxClass = -1; -SHAREDLIBEXPORT int dCCylinderClass = -1; -SHAREDLIBEXPORT int dPlaneClass = -1; - - -static dColliderFn * dSphereColliderFn (int num) -{ - if (num == dSphereClass) return (dColliderFn *) &dCollideSS; - if (num == dBoxClass) return (dColliderFn *) &dCollideSB; - if (num == dPlaneClass) return (dColliderFn *) &dCollideSP; - return 0; -} - - -static void dSphereAABB (dxGeom *geom, dReal aabb[6]) -{ - dxSphere *s = (dxSphere*) CLASSDATA(geom); - aabb[0] = geom->pos[0] - s->radius; - aabb[1] = geom->pos[0] + s->radius; - aabb[2] = geom->pos[1] - s->radius; - aabb[3] = geom->pos[1] + s->radius; - aabb[4] = geom->pos[2] - s->radius; - aabb[5] = geom->pos[2] + s->radius; -} - - -static dColliderFn * dBoxColliderFn (int num) -{ - if (num == dBoxClass) return (dColliderFn *) &dCollideBB; - if (num == dPlaneClass) return (dColliderFn *) &dCollideBP; - return 0; -} - - -static void dBoxAABB (dxGeom *geom, dReal aabb[6]) -{ - dxBox *b = (dxBox*) CLASSDATA(geom); - dReal xrange = REAL(0.5) * (dFabs (geom->R[0] * b->side[0]) + - dFabs (geom->R[1] * b->side[1]) + dFabs (geom->R[2] * b->side[2])); - dReal yrange = REAL(0.5) * (dFabs (geom->R[4] * b->side[0]) + - dFabs (geom->R[5] * b->side[1]) + dFabs (geom->R[6] * b->side[2])); - dReal zrange = REAL(0.5) * (dFabs (geom->R[8] * b->side[0]) + - dFabs (geom->R[9] * b->side[1]) + dFabs (geom->R[10] * b->side[2])); - aabb[0] = geom->pos[0] - xrange; - aabb[1] = geom->pos[0] + xrange; - aabb[2] = geom->pos[1] - yrange; - aabb[3] = geom->pos[1] + yrange; - aabb[4] = geom->pos[2] - zrange; - aabb[5] = geom->pos[2] + zrange; -} - - -static dColliderFn * dCCylinderColliderFn (int num) -{ - if (num == dSphereClass) return (dColliderFn *) &dCollideCS; - if (num == dPlaneClass) return (dColliderFn *) &dCollideCP; - if (num == dCCylinderClass) return (dColliderFn *) &dCollideCC; - if (num == dBoxClass) return (dColliderFn *) &dCollideCB; - return 0; -} - - -static void dCCylinderAABB (dxGeom *geom, dReal aabb[6]) -{ - dxCCylinder *c = (dxCCylinder*) CLASSDATA(geom); - dReal xrange = dFabs(geom->R[2] * c->lz) * REAL(0.5) + c->radius; - dReal yrange = dFabs(geom->R[6] * c->lz) * REAL(0.5) + c->radius; - dReal zrange = dFabs(geom->R[10] * c->lz) * REAL(0.5) + c->radius; - aabb[0] = geom->pos[0] - xrange; - aabb[1] = geom->pos[0] + xrange; - aabb[2] = geom->pos[1] - yrange; - aabb[3] = geom->pos[1] + yrange; - aabb[4] = geom->pos[2] - zrange; - aabb[5] = geom->pos[2] + zrange; -} - - -dColliderFn * dPlaneColliderFn (int num) -{ - return 0; -} - - -static void dPlaneAABB (dxGeom *geom, dReal aabb[6]) -{ - // @@@ planes that have normal vectors aligned along an axis can use a - // @@@ less comprehensive bounding box. - aabb[0] = -dInfinity; - aabb[1] = dInfinity; - aabb[2] = -dInfinity; - aabb[3] = dInfinity; - aabb[4] = -dInfinity; - aabb[5] = dInfinity; -} - - -dxGeom *dCreateSphere (dSpaceID space, dReal radius) -{ - dAASSERT (radius > 0); - if (dSphereClass == -1) { - dGeomClass c; - c.bytes = sizeof (dxSphere); - c.collider = &dSphereColliderFn; - c.aabb = &dSphereAABB; - c.aabb_test = 0; - c.dtor = 0; - dSphereClass = dCreateGeomClass (&c); - } - - dxGeom *g = dCreateGeom (dSphereClass); - if (space) dSpaceAdd (space,g); - dxSphere *s = (dxSphere*) CLASSDATA(g); - s->radius = radius; - return g; -} - - -dxGeom *dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz) -{ - dAASSERT (lx > 0 && ly > 0 && lz > 0); - if (dBoxClass == -1) { - dGeomClass c; - c.bytes = sizeof (dxBox); - c.collider = &dBoxColliderFn; - c.aabb = &dBoxAABB; - c.aabb_test = 0; - c.dtor = 0; - dBoxClass = dCreateGeomClass (&c); - } - - dxGeom *g = dCreateGeom (dBoxClass); - if (space) dSpaceAdd (space,g); - dxBox *b = (dxBox*) CLASSDATA(g); - b->side[0] = lx; - b->side[1] = ly; - b->side[2] = lz; - return g; -} - - -dxGeom * dCreateCCylinder (dSpaceID space, dReal radius, dReal length) -{ - dAASSERT (radius > 0 && length > 0); - if (dCCylinderClass == -1) { - dGeomClass c; - c.bytes = sizeof (dxCCylinder); - c.collider = &dCCylinderColliderFn; - c.aabb = &dCCylinderAABB; - c.aabb_test = 0; - c.dtor = 0; - dCCylinderClass = dCreateGeomClass (&c); - } - - dxGeom *g = dCreateGeom (dCCylinderClass); - if (space) dSpaceAdd (space,g); - dxCCylinder *c = (dxCCylinder*) CLASSDATA(g); - c->radius = radius; - c->lz = length; - return g; -} - - -dxGeom *dCreatePlane (dSpaceID space, - dReal a, dReal b, dReal c, dReal d) -{ - if (dPlaneClass == -1) { - dGeomClass c; - c.bytes = sizeof (dxPlane); - c.collider = &dPlaneColliderFn; - c.aabb = &dPlaneAABB; - c.aabb_test = 0; - c.dtor = 0; - dPlaneClass = dCreateGeomClass (&c); - } - - dxGeom *g = dCreateGeom (dPlaneClass); - if (space) dSpaceAdd (space,g); - dxPlane *p = (dxPlane*) CLASSDATA(g); - - // make sure plane normal has unit length - dReal l = a*a + b*b + c*c; - if (l > 0) { - l = dRecipSqrt(l); - p->p[0] = a*l; - p->p[1] = b*l; - p->p[2] = c*l; - p->p[3] = d*l; - } - else { - p->p[0] = 1; - p->p[1] = 0; - p->p[2] = 0; - p->p[3] = 0; - } - return g; -} - - -void dGeomSphereSetRadius (dGeomID g, dReal radius) -{ - dUASSERT (g && g->_class->num == dSphereClass,"argument not a sphere"); - dAASSERT (radius > 0); - dxSphere *s = (dxSphere*) CLASSDATA(g); - s->radius = radius; -} - - -void dGeomBoxSetLengths (dGeomID g, dReal lx, dReal ly, dReal lz) -{ - dUASSERT (g && g->_class->num == dBoxClass,"argument not a box"); - dAASSERT (lx > 0 && ly > 0 && lz > 0); - dxBox *b = (dxBox*) CLASSDATA(g); - b->side[0] = lx; - b->side[1] = ly; - b->side[2] = lz; -} - - -void dGeomPlaneSetParams (dGeomID g, dReal a, dReal b, dReal c, dReal d) -{ - dUASSERT (g && g->_class->num == dPlaneClass,"argument not a plane"); - dxPlane *p = (dxPlane*) CLASSDATA(g); - p->p[0] = a; - p->p[1] = b; - p->p[2] = c; - p->p[3] = d; -} - - -void dGeomCCylinderSetParams (dGeomID g, dReal radius, dReal length) -{ - dUASSERT (g && g->_class->num == dCCylinderClass,"argument not a ccylinder"); - dAASSERT (radius > 0 && length > 0); - dxCCylinder *c = (dxCCylinder*) CLASSDATA(g); - c->radius = radius; - c->lz = length; -} - - -dReal dGeomSphereGetRadius (dGeomID g) -{ - dUASSERT (g && g->_class->num == dSphereClass,"argument not a sphere"); - dxSphere *s = (dxSphere*) CLASSDATA(g); - return s->radius; -} - - -void dGeomBoxGetLengths (dGeomID g, dVector3 result) -{ - dUASSERT (g && g->_class->num == dBoxClass,"argument not a box"); - dxBox *b = (dxBox*) CLASSDATA(g); - result[0] = b->side[0]; - result[1] = b->side[1]; - result[2] = b->side[2]; -} - - -void dGeomPlaneGetParams (dGeomID g, dVector4 result) -{ - dUASSERT (g && g->_class->num == dPlaneClass,"argument not a plane"); - dxPlane *p = (dxPlane*) CLASSDATA(g); - result[0] = p->p[0]; - result[1] = p->p[1]; - result[2] = p->p[2]; - result[3] = p->p[3]; -} - - -void dGeomCCylinderGetParams (dGeomID g, dReal *radius, dReal *length) -{ - dUASSERT (g && g->_class->num == dCCylinderClass,"argument not a ccylinder"); - dxCCylinder *c = (dxCCylinder*) CLASSDATA(g); - *radius = c->radius; - *length = c->lz; -} - -//**************************************************************************** -// geom group - -int dGeomGroupClass = -1; - -static dColliderFn * dGeomGroupColliderFn (int num) -{ - return (dColliderFn *) &dCollideG; -} - - -static void dGeomGroupAABB (dxGeom *geom, dReal aabb[6]) -{ - dxGeomGroup *gr = (dxGeomGroup*) CLASSDATA(geom); - aabb[0] = dInfinity; - aabb[1] = -dInfinity; - aabb[2] = dInfinity; - aabb[3] = -dInfinity; - aabb[4] = dInfinity; - aabb[5] = -dInfinity; - int i,j; - for (i=0; i < gr->parts.size(); i++) { - dReal aabb2[6]; - gr->parts[i]->_class->aabb (gr->parts[i],aabb2); - for (j=0; j<6; j += 2) if (aabb2[j] < aabb[j]) aabb[j] = aabb2[j]; - for (j=1; j<6; j += 2) if (aabb2[j] > aabb[j]) aabb[j] = aabb2[j]; - } -} - - -static void dGeomGroupDtor (dxGeom *geom) -{ - dxGeomGroup *gr = (dxGeomGroup*) CLASSDATA(geom); - gr->parts.~dArray(); -} - - -dxGeom *dCreateGeomGroup (dSpaceID space) -{ - if (dGeomGroupClass == -1) { - dGeomClass c; - c.bytes = sizeof (dxGeomGroup); - c.collider = &dGeomGroupColliderFn; - c.aabb = &dGeomGroupAABB; - c.aabb_test = 0; - c.dtor = &dGeomGroupDtor; - dGeomGroupClass = dCreateGeomClass (&c); - } - - dxGeom *g = dCreateGeom (dGeomGroupClass); - if (space) dSpaceAdd (space,g); - dxGeomGroup *gr = (dxGeomGroup*) CLASSDATA(g); - gr->parts.constructor(); - return g; -} - - -void dGeomGroupAdd (dxGeom *g, dxGeom *x) -{ - dUASSERT (g && g->_class->num == dGeomGroupClass,"argument not a geomgroup"); - dxGeomGroup *gr = (dxGeomGroup*) CLASSDATA(g); - gr->parts.push (x); -} - - -void dGeomGroupRemove (dxGeom *g, dxGeom *x) -{ - dUASSERT (g && g->_class->num == dGeomGroupClass,"argument not a geomgroup"); - dxGeomGroup *gr = (dxGeomGroup*) CLASSDATA(g); - for (int i=0; i < gr->parts.size(); i++) { - if (gr->parts[i] == x) { - gr->parts.remove (i); - return; - } - } -} - - -int dGeomGroupGetNumGeoms (dxGeom *g) -{ - dUASSERT (g && g->_class->num == dGeomGroupClass,"argument not a geomgroup"); - dxGeomGroup *gr = (dxGeomGroup*) CLASSDATA(g); - return gr->parts.size(); -} - - -dxGeom * dGeomGroupGetGeom (dxGeom *g, int i) -{ - dUASSERT (g && g->_class->num == dGeomGroupClass,"argument not a geomgroup"); - dxGeomGroup *gr = (dxGeomGroup*) CLASSDATA(g); - dAASSERT (i >= 0 && i < gr->parts.size()); - return gr->parts[i]; -} - -//**************************************************************************** -// transformed geom - -int dGeomTransformClass = -1; - -struct dxGeomTransform { - dxGeom *obj; // object that is being transformed - int cleanup; // 1 to destroy obj when destroyed - int infomode; // 1 to put Tx geom in dContactGeom g1 - dVector3 final_pos; // final tx (body tx + relative tx) of the object. - dMatrix3 final_R; // this is only set if the AABB function is called -}; // by space collision before the collide fn is called - - -// compute final pos and R for the encapsulated geom object - -static void compute_final_tx (const dxGeom *g) -{ - dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(g); - dMULTIPLY0_331 (tr->final_pos,g->R,tr->obj->pos); - tr->final_pos[0] += g->pos[0]; - tr->final_pos[1] += g->pos[1]; - tr->final_pos[2] += g->pos[2]; - dMULTIPLY0_333 (tr->final_R,g->R,tr->obj->R); -} - - - -// this collides a transformed geom with another geom. the other geom can -// also be a transformed geom, but this case is not handled specially. - -int dCollideT (const dxGeom *o1, const dxGeom *o2, int flags, - dContactGeom *contact, int skip) -{ - dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(o1); - if (!tr->obj) return 0; - dUASSERT (tr->obj->spaceid==0, - "GeomTransform encapsulated object must not be in a space"); - dUASSERT (tr->obj->body==0, - "GeomTransform encapsulated object must not be attach to a body"); - - // backup the relative pos and R pointers of the encapsulated geom object, - // and the body pointer - dReal *posbak = tr->obj->pos; - dReal *Rbak = tr->obj->R; - dxBody *bodybak = tr->obj->body; - - // compute temporary pos and R for the encapsulated geom object - if (!o1->space_aabb) compute_final_tx (o1); - tr->obj->pos = tr->final_pos; - tr->obj->R = tr->final_R; - tr->obj->body = o1->body; - - // do the collision - int n = dCollide (tr->obj,const_cast<dxGeom*>(o2),flags,contact,skip); - - // if required, adjust the 'g1' values in the generated contacts so that - // thay indicated the GeomTransform object instead of the encapsulated - // object. - if (tr->infomode) { - for (int i=0; i<n; i++) { - dContactGeom *c = CONTACT(contact,skip*i); - c->g1 = const_cast<dxGeom*> (o1); - } - } - - // restore the pos, R and body - tr->obj->pos = posbak; - tr->obj->R = Rbak; - tr->obj->body = bodybak; - return n; -} - - -static dColliderFn * dGeomTransformColliderFn (int num) -{ - return (dColliderFn *) &dCollideT; -} - - -static void dGeomTransformAABB (dxGeom *geom, dReal aabb[6]) -{ - dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(geom); - if (!tr->obj) { - dSetZero (aabb,6); - return; - } - - // backup the relative pos and R pointers of the encapsulated geom object - dReal *posbak = tr->obj->pos; - dReal *Rbak = tr->obj->R; - - // compute temporary pos and R for the encapsulated geom object - compute_final_tx (geom); - tr->obj->pos = tr->final_pos; - tr->obj->R = tr->final_R; - - // compute the AABB - tr->obj->_class->aabb (tr->obj,aabb); - - // restore the pos and R - tr->obj->pos = posbak; - tr->obj->R = Rbak; -} - - -static void dGeomTransformDtor (dxGeom *geom) -{ - dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(geom); - if (tr->obj && tr->cleanup) { - dGeomDestroy (tr->obj); - } -} - - -dxGeom *dCreateGeomTransform (dSpaceID space) -{ - if (dGeomTransformClass == -1) { - dGeomClass c; - c.bytes = sizeof (dxGeomTransform); - c.collider = &dGeomTransformColliderFn; - c.aabb = &dGeomTransformAABB; - c.aabb_test = 0; - c.dtor = dGeomTransformDtor; - dGeomTransformClass = dCreateGeomClass (&c); - } - - dxGeom *g = dCreateGeom (dGeomTransformClass); - if (space) dSpaceAdd (space,g); - - dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(g); - tr->obj = 0; - tr->cleanup = 0; - tr->infomode = 0; - dSetZero (tr->final_pos,4); - dRSetIdentity (tr->final_R); - - return g; -} - - -void dGeomTransformSetGeom (dxGeom *g, dxGeom *obj) -{ - dUASSERT (g && g->_class->num == dGeomTransformClass, - "argument not a geom transform"); - dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(g); - if (tr->obj && tr->cleanup) { - dGeomDestroy (tr->obj); - } - tr->obj = obj; -} - - -dxGeom * dGeomTransformGetGeom (dxGeom *g) -{ - dUASSERT (g && g->_class->num == dGeomTransformClass, - "argument not a geom transform"); - dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(g); - return tr->obj; -} - - -void dGeomTransformSetCleanup (dGeomID g, int mode) -{ - dUASSERT (g && g->_class->num == dGeomTransformClass, - "argument not a geom transform"); - dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(g); - tr->cleanup = mode; -} - - -int dGeomTransformGetCleanup (dGeomID g) -{ - dUASSERT (g && g->_class->num == dGeomTransformClass, - "argument not a geom transform"); - dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(g); - return tr->cleanup; -} - - -void dGeomTransformSetInfo (dGeomID g, int mode) -{ - dUASSERT (g && g->_class->num == dGeomTransformClass, - "argument not a geom transform"); - dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(g); - tr->infomode = mode; -} - - -int dGeomTransformGetInfo (dGeomID g) -{ - dUASSERT (g && g->_class->num == dGeomTransformClass, - "argument not a geom transform"); - dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(g); - return tr->infomode; -} - -//**************************************************************************** -// other utility functions - -void dInfiniteAABB (dxGeom *geom, dReal aabb[6]) -{ - aabb[0] = -dInfinity; - aabb[1] = dInfinity; - aabb[2] = -dInfinity; - aabb[3] = dInfinity; - aabb[4] = -dInfinity; - aabb[5] = dInfinity; -} - - -void dCloseODE() -{ - if (colliders) { - delete colliders; - colliders = 0; - } - if (classes) { - for (int i=0; i < classes->size(); i++) { - dFree ((*classes)[i], sizeof (dxGeomClass)); - } - delete classes; - classes = 0; - } - - // reset geom class vars - dSphereClass = -1; - dBoxClass = -1; - dCCylinderClass = -1; - dPlaneClass = -1; - dGeomGroupClass = -1; - dGeomTransformClass = -1; - - // if you're using contrib code you may want to uncomment the following: - // dTriListClass = -1; - // dRayClass = -1; -} diff --git a/extern/ode/dist/ode/src/geom_internal.h b/extern/ode/dist/ode/src/geom_internal.h deleted file mode 100644 index 63be589ffe0..00000000000 --- a/extern/ode/dist/ode/src/geom_internal.h +++ /dev/null @@ -1,83 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#ifndef _ODE_GEOM_INTERNAL_H_ -#define _ODE_GEOM_INTERNAL_H_ - - -// mask for the number-of-contacts field in the dCollide() flags parameter -#define NUMC_MASK (0xffff) - - -// internal info for geometry class - -struct dxGeomClass { - dGetColliderFnFn *collider; - dGetAABBFn *aabb; - dAABBTestFn *aabb_test; - dGeomDtorFn *dtor; - int num; // class number - int size; // total size of object, including extra data area -}; - - -// position vector and rotation matrix for geometry objects that are not -// connected to bodies. - -struct dxPosR { - dVector3 pos; - dMatrix3 R; -}; - - -// common data for all geometry objects. the class-specific data area follows -// this structure. pos and R will either point to a separately allocated -// buffer (if body is 0 - pos points to the dxPosR object) or to the pos and -// R of the body (if body nonzero). - -struct dxGeom { // a dGeomID is a pointer to this - dxGeomClass *_class; // class of this object - void *data; // user data pointer - dBodyID body; // dynamics body associated with this object (if any) - dReal *pos; // pointer to object's position vector - dReal *R; // pointer to object's rotation matrix - dSpaceID spaceid; // the space this object is in - dGeomSpaceData space; // reserved for use by space this object is in - dReal *space_aabb; // ptr to aabb array held by dSpaceCollide() fn - // class-specific data follows here, with proper alignment. -}; - - -// this is the size of the dxGeom structure rounded up to a multiple of 16 -// bytes. any class specific data that comes after this will have the correct -// alignment. - -#define SIZEOF_DXGEOM dEFFICIENT_SIZE(sizeof(dxGeom)) - - -// given a pointer to a dxGeom, return a pointer to the class data that -// follows it. - -#define CLASSDATA(geomptr) (((char*)geomptr) + SIZEOF_DXGEOM) - -#endif - diff --git a/extern/ode/dist/ode/src/joint.cpp b/extern/ode/dist/ode/src/joint.cpp deleted file mode 100644 index 74e4c34cc71..00000000000 --- a/extern/ode/dist/ode/src/joint.cpp +++ /dev/null @@ -1,2160 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -/* - -design note: the general principle for giving a joint the option of connecting -to the static environment (i.e. the absolute frame) is to check the second -body (joint->node[1].body), and if it is zero then behave as if its body -transform is the identity. - -*/ - -#include <ode/odemath.h> -#include <ode/rotation.h> -#include <ode/matrix.h> -#include "joint.h" - -//**************************************************************************** -// externs - -extern "C" void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz); -extern "C" void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz); - -//**************************************************************************** -// utility - -// set three "ball-and-socket" rows in the constraint equation, and the -// corresponding right hand side. - -static inline void setBall (dxJoint *joint, dxJoint::Info2 *info, - dVector3 anchor1, dVector3 anchor2) -{ - // anchor points in global coordinates with respect to body PORs. - dVector3 a1,a2; - - int s = info->rowskip; - - // set jacobian - info->J1l[0] = 1; - info->J1l[s+1] = 1; - info->J1l[2*s+2] = 1; - dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1); - dCROSSMAT (info->J1a,a1,s,-,+); - if (joint->node[1].body) { - info->J2l[0] = -1; - info->J2l[s+1] = -1; - info->J2l[2*s+2] = -1; - dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2); - dCROSSMAT (info->J2a,a2,s,+,-); - } - - // set right hand side - dReal k = info->fps * info->erp; - if (joint->node[1].body) { - for (int j=0; j<3; j++) { - info->c[j] = k * (a2[j] + joint->node[1].body->pos[j] - - a1[j] - joint->node[0].body->pos[j]); - } - } - else { - for (int j=0; j<3; j++) { - info->c[j] = k * (anchor2[j] - a1[j] - - joint->node[0].body->pos[j]); - } - } -} - - -// this is like setBall(), except that `axis' is a unit length vector -// (in global coordinates) that should be used for the first jacobian -// position row (the other two row vectors will be derived from this). -// `erp1' is the erp value to use along the axis. - -static inline void setBall2 (dxJoint *joint, dxJoint::Info2 *info, - dVector3 anchor1, dVector3 anchor2, - dVector3 axis, dReal erp1) -{ - // anchor points in global coordinates with respect to body PORs. - dVector3 a1,a2; - - int i,s = info->rowskip; - - // get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0], - // [0 1 0] and [0 0 1], which makes everything much easier. - dVector3 q1,q2; - dPlaneSpace (axis,q1,q2); - - // set jacobian - for (i=0; i<3; i++) info->J1l[i] = axis[i]; - for (i=0; i<3; i++) info->J1l[s+i] = q1[i]; - for (i=0; i<3; i++) info->J1l[2*s+i] = q2[i]; - dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1); - dCROSS (info->J1a,=,a1,axis); - dCROSS (info->J1a+s,=,a1,q1); - dCROSS (info->J1a+2*s,=,a1,q2); - if (joint->node[1].body) { - for (i=0; i<3; i++) info->J2l[i] = -axis[i]; - for (i=0; i<3; i++) info->J2l[s+i] = -q1[i]; - for (i=0; i<3; i++) info->J2l[2*s+i] = -q2[i]; - dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2); - dCROSS (info->J2a,= -,a2,axis); - dCROSS (info->J2a+s,= -,a2,q1); - dCROSS (info->J2a+2*s,= -,a2,q2); - } - - // set right hand side - measure error along (axis,q1,q2) - dReal k1 = info->fps * erp1; - dReal k = info->fps * info->erp; - - for (i=0; i<3; i++) a1[i] += joint->node[0].body->pos[i]; - if (joint->node[1].body) { - for (i=0; i<3; i++) a2[i] += joint->node[1].body->pos[i]; - info->c[0] = k1 * (dDOT(axis,a2) - dDOT(axis,a1)); - info->c[1] = k * (dDOT(q1,a2) - dDOT(q1,a1)); - info->c[2] = k * (dDOT(q2,a2) - dDOT(q2,a1)); - } - else { - info->c[0] = k1 * (dDOT(axis,anchor2) - dDOT(axis,a1)); - info->c[1] = k * (dDOT(q1,anchor2) - dDOT(q1,a1)); - info->c[2] = k * (dDOT(q2,anchor2) - dDOT(q2,a1)); - } -} - - -// compute anchor points relative to bodies - -static void setAnchors (dxJoint *j, dReal x, dReal y, dReal z, - dVector3 anchor1, dVector3 anchor2) -{ - if (j->node[0].body) { - dReal q[4]; - q[0] = x - j->node[0].body->pos[0]; - q[1] = y - j->node[0].body->pos[1]; - q[2] = z - j->node[0].body->pos[2]; - q[3] = 0; - dMULTIPLY1_331 (anchor1,j->node[0].body->R,q); - if (j->node[1].body) { - q[0] = x - j->node[1].body->pos[0]; - q[1] = y - j->node[1].body->pos[1]; - q[2] = z - j->node[1].body->pos[2]; - q[3] = 0; - dMULTIPLY1_331 (anchor2,j->node[1].body->R,q); - } - else { - anchor2[0] = x; - anchor2[1] = y; - anchor2[2] = z; - } - } - anchor1[3] = 0; - anchor2[3] = 0; -} - - -// compute axes relative to bodies. axis2 can be 0 - -static void setAxes (dxJoint *j, dReal x, dReal y, dReal z, - dVector3 axis1, dVector3 axis2) -{ - if (j->node[0].body) { - dReal q[4]; - q[0] = x; - q[1] = y; - q[2] = z; - q[3] = 0; - dNormalize3 (q); - dMULTIPLY1_331 (axis1,j->node[0].body->R,q); - if (axis2) { - if (j->node[1].body) { - dMULTIPLY1_331 (axis2,j->node[1].body->R,q); - } - else { - axis2[0] = x; - axis2[1] = y; - axis2[2] = z; - } - axis2[3] = 0; - } - } - axis1[3] = 0; -} - - -static void getAnchor (dxJoint *j, dVector3 result, dVector3 anchor1) -{ - if (j->node[0].body) { - dMULTIPLY0_331 (result,j->node[0].body->R,anchor1); - result[0] += j->node[0].body->pos[0]; - result[1] += j->node[0].body->pos[1]; - result[2] += j->node[0].body->pos[2]; - } -} - - -static void getAxis (dxJoint *j, dVector3 result, dVector3 axis1) -{ - if (j->node[0].body) { - dMULTIPLY0_331 (result,j->node[0].body->R,axis1); - } -} - - -// given two bodies (body1,body2), the hinge axis that they are connected by -// w.r.t. body1 (axis), and the initial relative orientation between them -// (q_initial), return the relative rotation angle. the initial relative -// orientation corresponds to an angle of zero. if body2 is 0 then measure the -// angle between body1 and the static frame. -// -// this will not return the correct angle if the bodies rotate along any axis -// other than the given hinge axis. - -static dReal getHingeAngle (dxBody *body1, dxBody *body2, dVector3 axis, - dQuaternion q_initial) -{ - // the angle between the two bodies is extracted from the quaternion that - // represents the relative rotation between them. recall that a quaternion - // q is: - // [s,v] = [ cos(theta/2) , sin(theta/2) * u ] - // where s is a scalar and v is a 3-vector. u is a unit length axis and - // theta is a rotation along that axis. we can get theta/2 by: - // theta/2 = atan2 ( sin(theta/2) , cos(theta/2) ) - // but we can't get sin(theta/2) directly, only its absolute value, i.e.: - // |v| = |sin(theta/2)| * |u| - // = |sin(theta/2)| - // using this value will have a strange effect. recall that there are two - // quaternion representations of a given rotation, q and -q. typically as - // a body rotates along the axis it will go through a complete cycle using - // one representation and then the next cycle will use the other - // representation. this corresponds to u pointing in the direction of the - // hinge axis and then in the opposite direction. the result is that theta - // will appear to go "backwards" every other cycle. here is a fix: if u - // points "away" from the direction of the hinge (motor) axis (i.e. more - // than 90 degrees) then use -q instead of q. this represents the same - // rotation, but results in the cos(theta/2) value being sign inverted. - - // get qrel = relative rotation between the two bodies - dQuaternion qrel; - if (body2) { - dQuaternion qq; - dQMultiply1 (qq,body1->q,body2->q); - dQMultiply2 (qrel,qq,q_initial); - } - else { - // pretend body2->q is the identity - dQMultiply3 (qrel,body1->q,q_initial); - } - - // extract the angle from the quaternion. cost2 = cos(theta/2), - // sint2 = |sin(theta/2)| - dReal cost2 = qrel[0]; - dReal sint2 = dSqrt (qrel[1]*qrel[1]+qrel[2]*qrel[2]+qrel[3]*qrel[3]); - dReal theta = (dDOT(qrel+1,axis) >= 0) ? // @@@ padding assumptions - (2 * dAtan2(sint2,cost2)) : // if u points in direction of axis - (2 * dAtan2(sint2,-cost2)); // if u points in opposite direction - - // the angle we get will be between 0..2*pi, but we want to return angles - // between -pi..pi - if (theta > M_PI) theta -= 2*M_PI; - - // the angle we've just extracted has the wrong sign - theta = -theta; - - return theta; -} - -//**************************************************************************** -// dxJointLimitMotor - -void dxJointLimitMotor::init (dxWorld *world) -{ - vel = 0; - fmax = 0; - lostop = -dInfinity; - histop = dInfinity; - fudge_factor = 1; - normal_cfm = world->global_cfm; - stop_erp = world->global_erp; - stop_cfm = world->global_cfm; - bounce = 0; - limit = 0; - limit_err = 0; -} - - -void dxJointLimitMotor::set (int num, dReal value) -{ - switch (num) { - case dParamLoStop: - if (value <= histop) lostop = value; - break; - case dParamHiStop: - if (value >= lostop) histop = value; - break; - case dParamVel: - vel = value; - break; - case dParamFMax: - if (value >= 0) fmax = value; - break; - case dParamFudgeFactor: - if (value >= 0 && value <= 1) fudge_factor = value; - break; - case dParamBounce: - bounce = value; - break; - case dParamCFM: - normal_cfm = value; - break; - case dParamStopERP: - stop_erp = value; - break; - case dParamStopCFM: - stop_cfm = value; - break; - } -} - - -dReal dxJointLimitMotor::get (int num) -{ - switch (num) { - case dParamLoStop: return lostop; - case dParamHiStop: return histop; - case dParamVel: return vel; - case dParamFMax: return fmax; - case dParamFudgeFactor: return fudge_factor; - case dParamBounce: return bounce; - case dParamCFM: return normal_cfm; - case dParamStopERP: return stop_erp; - case dParamStopCFM: return stop_cfm; - default: return 0; - } -} - - -int dxJointLimitMotor::testRotationalLimit (dReal angle) -{ - if (angle <= lostop) { - limit = 1; - limit_err = angle - lostop; - return 1; - } - else if (angle >= histop) { - limit = 2; - limit_err = angle - histop; - return 1; - } - else { - limit = 0; - return 0; - } -} - - -int dxJointLimitMotor::addLimot (dxJoint *joint, - dxJoint::Info2 *info, int row, - dVector3 ax1, int rotational) -{ - int srow = row * info->rowskip; - - // if the joint is powered, or has joint limits, add in the extra row - int powered = fmax > 0; - if (powered || limit) { - dReal *J1 = rotational ? info->J1a : info->J1l; - dReal *J2 = rotational ? info->J2a : info->J2l; - - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - if (joint->node[1].body) { - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - } - - // if we're limited low and high simultaneously, the joint motor is - // ineffective - if (limit && (lostop == histop)) powered = 0; - - if (powered) { - info->cfm[row] = normal_cfm; - if (! limit) { - info->c[row] = vel; - info->lo[row] = -fmax; - info->hi[row] = fmax; - } - else { - // the joint is at a limit, AND is being powered. if the joint is - // being powered into the limit then we apply the maximum motor force - // in that direction, because the motor is working against the - // immovable limit. if the joint is being powered away from the limit - // then we have problems because actually we need *two* lcp - // constraints to handle this case. so we fake it and apply some - // fraction of the maximum force. the fraction to use can be set as - // a fudge factor. - - dReal fm = fmax; - if (vel > 0) fm = -fm; - - // if we're powering away from the limit, apply the fudge factor - if ((limit==1 && vel > 0) || (limit==2 && vel < 0)) fm *= fudge_factor; - - if (rotational) { - dBodyAddTorque (joint->node[0].body,-fm*ax1[0],-fm*ax1[1], - -fm*ax1[2]); - if (joint->node[1].body) - dBodyAddTorque (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]); - } - else { - dBodyAddForce (joint->node[0].body,-fm*ax1[0],-fm*ax1[1],-fm*ax1[2]); - if (joint->node[1].body) - dBodyAddForce (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]); - } - } - } - - if (limit) { - dReal k = info->fps * stop_erp; - info->c[row] = -k * limit_err; - info->cfm[row] = stop_cfm; - - if (lostop == histop) { - // limited low and high simultaneously - info->lo[row] = -dInfinity; - info->hi[row] = dInfinity; - } - else { - if (limit == 1) { - // low limit - info->lo[row] = 0; - info->hi[row] = dInfinity; - } - else { - // high limit - info->lo[row] = -dInfinity; - info->hi[row] = 0; - } - - // deal with bounce - if (bounce > 0) { - // calculate joint velocity - dReal vel; - if (rotational) { - vel = dDOT(joint->node[0].body->avel,ax1); - if (joint->node[1].body) - vel -= dDOT(joint->node[1].body->avel,ax1); - } - else { - vel = dDOT(joint->node[0].body->lvel,ax1); - if (joint->node[1].body) - vel -= dDOT(joint->node[1].body->lvel,ax1); - } - - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if (limit == 1) { - // low limit - if (vel < 0) { - dReal newc = -bounce * vel; - if (newc > info->c[row]) info->c[row] = newc; - } - } - else { - // high limit - all those computations are reversed - if (vel > 0) { - dReal newc = -bounce * vel; - if (newc < info->c[row]) info->c[row] = newc; - } - } - } - } - } - return 1; - } - else return 0; -} - -//**************************************************************************** -// ball and socket - -static void ballInit (dxJointBall *j) -{ - dSetZero (j->anchor1,4); - dSetZero (j->anchor2,4); -} - - -static void ballGetInfo1 (dxJointBall *j, dxJoint::Info1 *info) -{ - info->m = 3; - info->nub = 3; -} - - -static void ballGetInfo2 (dxJointBall *joint, dxJoint::Info2 *info) -{ - setBall (joint,info,joint->anchor1,joint->anchor2); -} - - -extern "C" void dJointSetBallAnchor (dxJointBall *joint, - dReal x, dReal y, dReal z) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); - setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); -} - - -extern "C" void dJointGetBallAnchor (dxJointBall *joint, dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); - getAnchor (joint,result,joint->anchor1); -} - - -dxJoint::Vtable __dball_vtable = { - sizeof(dxJointBall), - (dxJoint::init_fn*) ballInit, - (dxJoint::getInfo1_fn*) ballGetInfo1, - (dxJoint::getInfo2_fn*) ballGetInfo2, - dJointTypeBall}; - -//**************************************************************************** -// hinge - -static void hingeInit (dxJointHinge *j) -{ - dSetZero (j->anchor1,4); - dSetZero (j->anchor2,4); - dSetZero (j->axis1,4); - j->axis1[0] = 1; - dSetZero (j->axis2,4); - j->axis2[0] = 1; - dSetZero (j->qrel,4); - j->limot.init (j->world); -} - - -static void hingeGetInfo1 (dxJointHinge *j, dxJoint::Info1 *info) -{ - info->nub = 5; - - // see if joint is powered - if (j->limot.fmax > 0) - info->m = 6; // powered hinge needs an extra constraint row - else info->m = 5; - - // see if we're at a joint limit. - if ((j->limot.lostop >= -M_PI || j->limot.histop <= M_PI) && - j->limot.lostop <= j->limot.histop) { - dReal angle = getHingeAngle (j->node[0].body,j->node[1].body,j->axis1, - j->qrel); - if (j->limot.testRotationalLimit (angle)) info->m = 6; - } -} - - -static void hingeGetInfo2 (dxJointHinge *joint, dxJoint::Info2 *info) -{ - // set the three ball-and-socket rows - setBall (joint,info,joint->anchor1,joint->anchor2); - - // set the two hinge rows. the hinge axis should be the only unconstrained - // rotational axis, the angular velocity of the two bodies perpendicular to - // the hinge axis should be equal. thus the constraint equations are - // p*w1 - p*w2 = 0 - // q*w1 - q*w2 = 0 - // where p and q are unit vectors normal to the hinge axis, and w1 and w2 - // are the angular velocity vectors of the two bodies. - - dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body - dVector3 p,q; // plane space vectors for ax1 - dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); - dPlaneSpace (ax1,p,q); - - int s3=3*info->rowskip; - int s4=4*info->rowskip; - - info->J1a[s3+0] = p[0]; - info->J1a[s3+1] = p[1]; - info->J1a[s3+2] = p[2]; - info->J1a[s4+0] = q[0]; - info->J1a[s4+1] = q[1]; - info->J1a[s4+2] = q[2]; - - if (joint->node[1].body) { - info->J2a[s3+0] = -p[0]; - info->J2a[s3+1] = -p[1]; - info->J2a[s3+2] = -p[2]; - info->J2a[s4+0] = -q[0]; - info->J2a[s4+1] = -q[1]; - info->J2a[s4+2] = -q[2]; - } - - // compute the right hand side of the constraint equation. set relative - // body velocities along p and q to bring the hinge back into alignment. - // if ax1,ax2 are the unit length hinge axes as computed from body1 and - // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). - // if `theta' is the angle between ax1 and ax2, we need an angular velocity - // along u to cover angle erp*theta in one step : - // |angular_velocity| = angle/time = erp*theta / stepsize - // = (erp*fps) * theta - // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| - // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) - // ...as ax1 and ax2 are unit length. if theta is smallish, - // theta ~= sin(theta), so - // angular_velocity = (erp*fps) * (ax1 x ax2) - // ax1 x ax2 is in the plane space of ax1, so we project the angular - // velocity to p and q to find the right hand side. - - dVector3 ax2,b; - if (joint->node[1].body) { - dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); - } - else { - ax2[0] = joint->axis2[0]; - ax2[1] = joint->axis2[1]; - ax2[2] = joint->axis2[2]; - } - dCROSS (b,=,ax1,ax2); - dReal k = info->fps * info->erp; - info->c[3] = k * dDOT(b,p); - info->c[4] = k * dDOT(b,q); - - // if the hinge is powered, or has joint limits, add in the stuff - joint->limot.addLimot (joint,info,5,ax1,1); -} - - -// compute initial relative rotation body1 -> body2, or env -> body1 - -static void hingeComputeInitialRelativeRotation (dxJointHinge *joint) -{ - if (joint->node[0].body) { - if (joint->node[1].body) { - dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); - } - else { - // set joint->qrel to the transpose of the first body q - joint->qrel[0] = joint->node[0].body->q[0]; - for (int i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; - } - } -} - - -extern "C" void dJointSetHingeAnchor (dxJointHinge *joint, - dReal x, dReal y, dReal z) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); - setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); - hingeComputeInitialRelativeRotation (joint); -} - - -extern "C" void dJointSetHingeAxis (dxJointHinge *joint, - dReal x, dReal y, dReal z) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); - setAxes (joint,x,y,z,joint->axis1,joint->axis2); - hingeComputeInitialRelativeRotation (joint); -} - - -extern "C" void dJointGetHingeAnchor (dxJointHinge *joint, dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); - getAnchor (joint,result,joint->anchor1); -} - - -extern "C" void dJointGetHingeAxis (dxJointHinge *joint, dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); - getAxis (joint,result,joint->axis1); -} - - -extern "C" void dJointSetHingeParam (dxJointHinge *joint, - int parameter, dReal value) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); - joint->limot.set (parameter,value); -} - - -extern "C" dReal dJointGetHingeParam (dxJointHinge *joint, int parameter) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); - return joint->limot.get (parameter); -} - - -extern "C" dReal dJointGetHingeAngle (dxJointHinge *joint) -{ - dAASSERT(joint); - dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); - if (joint->node[0].body) { - return getHingeAngle (joint->node[0].body,joint->node[1].body,joint->axis1, - joint->qrel); - } - else return 0; -} - - -extern "C" dReal dJointGetHingeAngleRate (dxJointHinge *joint) -{ - dAASSERT(joint); - dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge"); - if (joint->node[0].body) { - dVector3 axis; - dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1); - dReal rate = dDOT(axis,joint->node[0].body->avel); - if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); - return rate; - } - else return 0; -} - - -dxJoint::Vtable __dhinge_vtable = { - sizeof(dxJointHinge), - (dxJoint::init_fn*) hingeInit, - (dxJoint::getInfo1_fn*) hingeGetInfo1, - (dxJoint::getInfo2_fn*) hingeGetInfo2, - dJointTypeHinge}; - -//**************************************************************************** -// slider - -static void sliderInit (dxJointSlider *j) -{ - dSetZero (j->axis1,4); - j->axis1[0] = 1; - dSetZero (j->qrel,4); - dSetZero (j->offset,4); - j->limot.init (j->world); -} - - -extern "C" dReal dJointGetSliderPosition (dxJointSlider *joint) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); - - // get axis1 in global coordinates - dVector3 ax1,q; - dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); - - if (joint->node[1].body) { - // get body2 + offset point in global coordinates - dMULTIPLY0_331 (q,joint->node[1].body->R,joint->offset); - for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] - q[i] - - joint->node[1].body->pos[i]; - } - else { - for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] - - joint->offset[i]; - - } - return dDOT(ax1,q); -} - - -extern "C" dReal dJointGetSliderPositionRate (dxJointSlider *joint) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); - - // get axis1 in global coordinates - dVector3 ax1; - dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); - - if (joint->node[1].body) { - return dDOT(ax1,joint->node[0].body->lvel) - - dDOT(ax1,joint->node[1].body->lvel); - } - else { - return dDOT(ax1,joint->node[0].body->lvel); - } -} - - -static void sliderGetInfo1 (dxJointSlider *j, dxJoint::Info1 *info) -{ - info->nub = 5; - - // see if joint is powered - if (j->limot.fmax > 0) - info->m = 6; // powered slider needs an extra constraint row - else info->m = 5; - - // see if we're at a joint limit. - j->limot.limit = 0; - if ((j->limot.lostop > -dInfinity || j->limot.histop < dInfinity) && - j->limot.lostop <= j->limot.histop) { - // measure joint position - dReal pos = dJointGetSliderPosition (j); - if (pos <= j->limot.lostop) { - j->limot.limit = 1; - j->limot.limit_err = pos - j->limot.lostop; - info->m = 6; - } - else if (pos >= j->limot.histop) { - j->limot.limit = 2; - j->limot.limit_err = pos - j->limot.histop; - info->m = 6; - } - } -} - - -static void sliderGetInfo2 (dxJointSlider *joint, dxJoint::Info2 *info) -{ - int i,s = info->rowskip; - int s2=2*s,s3=3*s,s4=4*s; - - // pull out pos and R for both bodies. also get the `connection' - // vector pos2-pos1. - - dReal *pos1,*pos2,*R1,*R2; - dVector3 c; - pos1 = joint->node[0].body->pos; - R1 = joint->node[0].body->R; - if (joint->node[1].body) { - pos2 = joint->node[1].body->pos; - R2 = joint->node[1].body->R; - for (i=0; i<3; i++) c[i] = pos2[i] - pos1[i]; - } - else { - pos2 = 0; - R2 = 0; - } - - // 3 rows to make body rotations equal - info->J1a[0] = 1; - info->J1a[s+1] = 1; - info->J1a[s2+2] = 1; - if (joint->node[1].body) { - info->J2a[0] = -1; - info->J2a[s+1] = -1; - info->J2a[s2+2] = -1; - } - - // remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would - // result in three equations, so we project along the planespace vectors - // so that sliding along the slider axis is disregarded. for symmetry we - // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2. - - dVector3 ax1; // joint axis in global coordinates (unit length) - dVector3 p,q; // plane space of ax1 - dMULTIPLY0_331 (ax1,R1,joint->axis1); - dPlaneSpace (ax1,p,q); - if (joint->node[1].body) { - dVector3 tmp; - dCROSS (tmp, = REAL(0.5) * ,c,p); - for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; - for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; - dCROSS (tmp, = REAL(0.5) * ,c,q); - for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i]; - for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i]; - for (i=0; i<3; i++) info->J2l[s3+i] = -p[i]; - for (i=0; i<3; i++) info->J2l[s4+i] = -q[i]; - } - for (i=0; i<3; i++) info->J1l[s3+i] = p[i]; - for (i=0; i<3; i++) info->J1l[s4+i] = q[i]; - - // compute the right hand side. the first three elements will result in - // relative angular velocity of the two bodies - this is set to bring them - // back into alignment. the correcting angular velocity is - // |angular_velocity| = angle/time = erp*theta / stepsize - // = (erp*fps) * theta - // angular_velocity = |angular_velocity| * u - // = (erp*fps) * theta * u - // where rotation along unit length axis u by theta brings body 2's frame - // to qrel with respect to body 1's frame. using a small angle approximation - // for sin(), this gives - // angular_velocity = (erp*fps) * 2 * v - // where the quaternion of the relative rotation between the two bodies is - // q = [cos(theta/2) sin(theta/2)*u] = [s v] - - // get qerr = relative rotation (rotation error) between two bodies - dQuaternion qerr,e; - if (joint->node[1].body) { - dQuaternion qq; - dQMultiply1 (qq,joint->node[0].body->q,joint->node[1].body->q); - dQMultiply2 (qerr,qq,joint->qrel); - } - else { - dQMultiply3 (qerr,joint->node[0].body->q,joint->qrel); - } - if (qerr[0] < 0) { - qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small - qerr[2] = -qerr[2]; - qerr[3] = -qerr[3]; - } - dMULTIPLY0_331 (e,joint->node[0].body->R,qerr+1); // @@@ bad SIMD padding! - dReal k = info->fps * info->erp; - info->c[0] = 2*k * e[0]; - info->c[1] = 2*k * e[1]; - info->c[2] = 2*k * e[2]; - - // compute last two elements of right hand side. we want to align the offset - // point (in body 2's frame) with the center of body 1. - if (joint->node[1].body) { - dVector3 ofs; // offset point in global coordinates - dMULTIPLY0_331 (ofs,R2,joint->offset); - for (i=0; i<3; i++) c[i] += ofs[i]; - info->c[3] = k * dDOT(p,c); - info->c[4] = k * dDOT(q,c); - } - else { - dVector3 ofs; // offset point in global coordinates - for (i=0; i<3; i++) ofs[i] = joint->offset[i] - pos1[i]; - info->c[3] = k * dDOT(p,ofs); - info->c[4] = k * dDOT(q,ofs); - } - - // if the slider is powered, or has joint limits, add in the extra row - joint->limot.addLimot (joint,info,5,ax1,0); -} - - -extern "C" void dJointSetSliderAxis (dxJointSlider *joint, - dReal x, dReal y, dReal z) -{ - int i; - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); - setAxes (joint,x,y,z,joint->axis1,0); - - // compute initial relative rotation body1 -> body2, or env -> body1 - // also compute center of body1 w.r.t body 2 - if (joint->node[1].body) { - dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); - dVector3 c; - for (i=0; i<3; i++) - c[i] = joint->node[0].body->pos[i] - joint->node[1].body->pos[i]; - dMULTIPLY1_331 (joint->offset,joint->node[1].body->R,c); - } - else { - // set joint->qrel to the transpose of the first body's q - joint->qrel[0] = joint->node[0].body->q[0]; - for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; - for (i=0; i<3; i++) joint->offset[i] = joint->node[0].body->pos[i]; - } -} - - -extern "C" void dJointGetSliderAxis (dxJointSlider *joint, dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); - getAxis (joint,result,joint->axis1); -} - - -extern "C" void dJointSetSliderParam (dxJointSlider *joint, - int parameter, dReal value) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); - joint->limot.set (parameter,value); -} - - -extern "C" dReal dJointGetSliderParam (dxJointSlider *joint, int parameter) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); - return joint->limot.get (parameter); -} - - -dxJoint::Vtable __dslider_vtable = { - sizeof(dxJointSlider), - (dxJoint::init_fn*) sliderInit, - (dxJoint::getInfo1_fn*) sliderGetInfo1, - (dxJoint::getInfo2_fn*) sliderGetInfo2, - dJointTypeSlider}; - -//**************************************************************************** -// contact - -static void contactInit (dxJointContact *j) -{ - // default frictionless contact. hmmm, this info gets overwritten straight - // away anyway, so why bother? - j->contact.surface.mode = 0; - j->contact.surface.mu = 0; - dSetZero (j->contact.geom.pos,4); - dSetZero (j->contact.geom.normal,4); - j->contact.geom.depth = 0; -} - - -static void contactGetInfo1 (dxJointContact *j, dxJoint::Info1 *info) -{ - // make sure mu's >= 0, then calculate number of constraint rows and number - // of unbounded rows. - int m = 1, nub=0; - if (j->contact.surface.mu < 0) j->contact.surface.mu = 0; - if (j->contact.surface.mode & dContactMu2) { - if (j->contact.surface.mu > 0) m++; - if (j->contact.surface.mu2 < 0) j->contact.surface.mu2 = 0; - if (j->contact.surface.mu2 > 0) m++; - if (j->contact.surface.mu == dInfinity) nub ++; - if (j->contact.surface.mu2 == dInfinity) nub ++; - } - else { - if (j->contact.surface.mu > 0) m += 2; - if (j->contact.surface.mu == dInfinity) nub += 2; - } - - j->the_m = m; - info->m = m; - info->nub = nub; -} - - -static void contactGetInfo2 (dxJointContact *j, dxJoint::Info2 *info) -{ - int i,s = info->rowskip; - int s2 = 2*s; - - // get normal, with sign adjusted for body1/body2 polarity - dVector3 normal; - if (j->flags & dJOINT_REVERSE) { - normal[0] = j->contact.geom.normal[0]; - normal[1] = j->contact.geom.normal[1]; - normal[2] = j->contact.geom.normal[2]; - } - else { - normal[0] = - j->contact.geom.normal[0]; - normal[1] = - j->contact.geom.normal[1]; - normal[2] = - j->contact.geom.normal[2]; - } - normal[3] = 0; // @@@ hmmm - - // c1,c2 = contact points with respect to body PORs - dVector3 c1,c2; - for (i=0; i<3; i++) c1[i] = j->contact.geom.pos[i] - j->node[0].body->pos[i]; - - // set jacobian for normal - info->J1l[0] = normal[0]; - info->J1l[1] = normal[1]; - info->J1l[2] = normal[2]; - dCROSS (info->J1a,=,c1,normal); - if (j->node[1].body) { - for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] - - j->node[1].body->pos[i]; - info->J2l[0] = -normal[0]; - info->J2l[1] = -normal[1]; - info->J2l[2] = -normal[2]; - dCROSS (info->J2a,= -,c2,normal); - } - - // set right hand side and cfm value for normal - dReal erp = info->erp; - if (j->contact.surface.mode & dContactSoftERP) - erp = j->contact.surface.soft_erp; - dReal k = info->fps * erp; - info->c[0] = k*j->contact.geom.depth; - if (j->contact.surface.mode & dContactSoftCFM) - info->cfm[0] = j->contact.surface.soft_cfm; - - // deal with bounce - if (j->contact.surface.mode & dContactBounce) { - // calculate outgoing velocity (-ve for incoming contact) - dReal outgoing = dDOT(info->J1l,j->node[0].body->lvel) + - dDOT(info->J1a,j->node[0].body->avel); - if (j->node[1].body) { - outgoing += dDOT(info->J2l,j->node[1].body->lvel) + - dDOT(info->J2a,j->node[1].body->avel); - } - // only apply bounce if the outgoing velocity is greater than the - // threshold, and if the resulting c[0] exceeds what we already have. - if (j->contact.surface.bounce_vel >= 0 && - (-outgoing) > j->contact.surface.bounce_vel) { - dReal newc = - j->contact.surface.bounce * outgoing; - if (newc > info->c[0]) info->c[0] = newc; - } - } - - // set LCP limits for normal - info->lo[0] = 0; - info->hi[0] = dInfinity; - - // now do jacobian for tangential forces - dVector3 t1,t2; // two vectors tangential to normal - - // first friction direction - if (j->the_m >= 2) { - if (j->contact.surface.mode & dContactFDir1) { // use fdir1 ? - t1[0] = j->contact.fdir1[0]; - t1[1] = j->contact.fdir1[1]; - t1[2] = j->contact.fdir1[2]; - dCROSS (t2,=,normal,t1); - } - else { - dPlaneSpace (normal,t1,t2); - } - info->J1l[s+0] = t1[0]; - info->J1l[s+1] = t1[1]; - info->J1l[s+2] = t1[2]; - dCROSS (info->J1a+s,=,c1,t1); - if (j->node[1].body) { - info->J2l[s+0] = -t1[0]; - info->J2l[s+1] = -t1[1]; - info->J2l[s+2] = -t1[2]; - dCROSS (info->J2a+s,= -,c2,t1); - } - // set right hand side - if (j->contact.surface.mode & dContactMotion1) { - info->c[1] = j->contact.surface.motion1; - } - // set LCP bounds and friction index. this depends on the approximation - // mode - info->lo[1] = -j->contact.surface.mu; - info->hi[1] = j->contact.surface.mu; - if (j->contact.surface.mode & dContactApprox1_1) info->findex[1] = 0; - - // set slip (constraint force mixing) - if (j->contact.surface.mode & dContactSlip1) - info->cfm[1] = j->contact.surface.slip1; - } - - // second friction direction - if (j->the_m >= 3) { - info->J1l[s2+0] = t2[0]; - info->J1l[s2+1] = t2[1]; - info->J1l[s2+2] = t2[2]; - dCROSS (info->J1a+s2,=,c1,t2); - if (j->node[1].body) { - info->J2l[s2+0] = -t2[0]; - info->J2l[s2+1] = -t2[1]; - info->J2l[s2+2] = -t2[2]; - dCROSS (info->J2a+s2,= -,c2,t2); - } - // set right hand side - if (j->contact.surface.mode & dContactMotion2) { - info->c[2] = j->contact.surface.motion2; - } - // set LCP bounds and friction index. this depends on the approximation - // mode - if (j->contact.surface.mode & dContactMu2) { - info->lo[2] = -j->contact.surface.mu2; - info->hi[2] = j->contact.surface.mu2; - } - else { - info->lo[2] = -j->contact.surface.mu; - info->hi[2] = j->contact.surface.mu; - } - if (j->contact.surface.mode & dContactApprox1_2) info->findex[2] = 0; - - // set slip (constraint force mixing) - if (j->contact.surface.mode & dContactSlip2) - info->cfm[2] = j->contact.surface.slip2; - } -} - - -dxJoint::Vtable __dcontact_vtable = { - sizeof(dxJointContact), - (dxJoint::init_fn*) contactInit, - (dxJoint::getInfo1_fn*) contactGetInfo1, - (dxJoint::getInfo2_fn*) contactGetInfo2, - dJointTypeContact}; - -//**************************************************************************** -// hinge 2. note that this joint must be attached to two bodies for it to work - -static dReal measureHinge2Angle (dxJointHinge2 *joint) -{ - dVector3 a1,a2; - dMULTIPLY0_331 (a1,joint->node[1].body->R,joint->axis2); - dMULTIPLY1_331 (a2,joint->node[0].body->R,a1); - dReal x = dDOT(joint->v1,a2); - dReal y = dDOT(joint->v2,a2); - return -dAtan2 (y,x); -} - - -static void hinge2Init (dxJointHinge2 *j) -{ - dSetZero (j->anchor1,4); - dSetZero (j->anchor2,4); - dSetZero (j->axis1,4); - j->axis1[0] = 1; - dSetZero (j->axis2,4); - j->axis2[1] = 1; - j->c0 = 0; - j->s0 = 0; - - dSetZero (j->v1,4); - j->v1[0] = 1; - dSetZero (j->v2,4); - j->v2[1] = 1; - - j->limot1.init (j->world); - j->limot2.init (j->world); - - j->susp_erp = j->world->global_erp; - j->susp_cfm = j->world->global_cfm; - - j->flags |= dJOINT_TWOBODIES; -} - - -static void hinge2GetInfo1 (dxJointHinge2 *j, dxJoint::Info1 *info) -{ - info->m = 4; - info->nub = 4; - - // see if we're powered or at a joint limit for axis 1 - int atlimit=0; - if ((j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) && - j->limot1.lostop <= j->limot1.histop) { - dReal angle = measureHinge2Angle (j); - if (j->limot1.testRotationalLimit (angle)) atlimit = 1; - } - if (atlimit || j->limot1.fmax > 0) info->m++; - - // see if we're powering axis 2 (we currently never limit this axis) - j->limot2.limit = 0; - if (j->limot2.fmax > 0) info->m++; -} - - -// macro that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are -// relative to body 1 and 2 initially) and then computes the constrained -// rotational axis as the cross product of ax1 and ax2. -// the sin and cos of the angle between axis 1 and 2 is computed, this comes -// from dot and cross product rules. - -#define HINGE2_GET_AXIS_INFO(axis,sin_angle,cos_angle) \ - dVector3 ax1,ax2; \ - dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); \ - dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); \ - dCROSS (axis,=,ax1,ax2); \ - sin_angle = dSqrt (axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); \ - cos_angle = dDOT (ax1,ax2); - - -static void hinge2GetInfo2 (dxJointHinge2 *joint, dxJoint::Info2 *info) -{ - // get information we need to set the hinge row - dReal s,c; - dVector3 q; - HINGE2_GET_AXIS_INFO (q,s,c); - dNormalize3 (q); // @@@ quicker: divide q by s ? - - // set the three ball-and-socket rows (aligned to the suspension axis ax1) - setBall2 (joint,info,joint->anchor1,joint->anchor2,ax1,joint->susp_erp); - - // set the hinge row - int s3=3*info->rowskip; - info->J1a[s3+0] = q[0]; - info->J1a[s3+1] = q[1]; - info->J1a[s3+2] = q[2]; - if (joint->node[1].body) { - info->J2a[s3+0] = -q[0]; - info->J2a[s3+1] = -q[1]; - info->J2a[s3+2] = -q[2]; - } - - // compute the right hand side for the constrained rotational DOF. - // axis 1 and axis 2 are separated by an angle `theta'. the desired - // separation angle is theta0. sin(theta0) and cos(theta0) are recorded - // in the joint structure. the correcting angular velocity is: - // |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize - // = (erp*fps) * (theta0-theta) - // (theta0-theta) can be computed using the following small-angle-difference - // approximation: - // theta0-theta ~= tan(theta0-theta) - // = sin(theta0-theta)/cos(theta0-theta) - // = (c*s0 - s*c0) / (c*c0 + s*s0) - // = c*s0 - s*c0 assuming c*c0 + s*s0 ~= 1 - // where c = cos(theta), s = sin(theta) - // c0 = cos(theta0), s0 = sin(theta0) - - dReal k = info->fps * info->erp; - info->c[3] = k * (joint->c0 * s - joint->s0 * c); - - // if the axis1 hinge is powered, or has joint limits, add in more stuff - int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1); - - // if the axis2 hinge is powered, add in more stuff - joint->limot2.addLimot (joint,info,row,ax2,1); - - // set parameter for the suspension - info->cfm[0] = joint->susp_cfm; -} - - -// compute vectors v1 and v2 (embedded in body1), used to measure angle -// between body 1 and body 2 - -static void makeHinge2V1andV2 (dxJointHinge2 *joint) -{ - if (joint->node[0].body) { - // get axis 1 and 2 in global coords - dVector3 ax1,ax2,v; - dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); - dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); - - // don't do anything if the axis1 or axis2 vectors are zero or the same - if ((ax1[0]==0 && ax1[1]==0 && ax1[2]==0) || - (ax2[0]==0 && ax2[1]==0 && ax2[2]==0) || - (ax1[0]==ax2[0] && ax1[1]==ax2[1] && ax1[2]==ax2[2])) return; - - // modify axis 2 so it's perpendicular to axis 1 - dReal k = dDOT(ax1,ax2); - for (int i=0; i<3; i++) ax2[i] -= k*ax1[i]; - dNormalize3 (ax2); - - // make v1 = modified axis2, v2 = axis1 x (modified axis2) - dCROSS (v,=,ax1,ax2); - dMULTIPLY1_331 (joint->v1,joint->node[0].body->R,ax2); - dMULTIPLY1_331 (joint->v2,joint->node[0].body->R,v); - } -} - - -extern "C" void dJointSetHinge2Anchor (dxJointHinge2 *joint, - dReal x, dReal y, dReal z) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); - setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); - makeHinge2V1andV2 (joint); -} - - -extern "C" void dJointSetHinge2Axis1 (dxJointHinge2 *joint, - dReal x, dReal y, dReal z) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); - if (joint->node[0].body) { - dReal q[4]; - q[0] = x; - q[1] = y; - q[2] = z; - q[3] = 0; - dNormalize3 (q); - dMULTIPLY1_331 (joint->axis1,joint->node[0].body->R,q); - joint->axis1[3] = 0; - - // compute the sin and cos of the angle between axis 1 and axis 2 - dVector3 ax; - HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0); - } - makeHinge2V1andV2 (joint); -} - - -extern "C" void dJointSetHinge2Axis2 (dxJointHinge2 *joint, - dReal x, dReal y, dReal z) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); - if (joint->node[1].body) { - dReal q[4]; - q[0] = x; - q[1] = y; - q[2] = z; - q[3] = 0; - dNormalize3 (q); - dMULTIPLY1_331 (joint->axis2,joint->node[1].body->R,q); - joint->axis1[3] = 0; - - // compute the sin and cos of the angle between axis 1 and axis 2 - dVector3 ax; - HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0); - } - makeHinge2V1andV2 (joint); -} - - -extern "C" void dJointSetHinge2Param (dxJointHinge2 *joint, - int parameter, dReal value) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); - if ((parameter & 0xff00) == 0x100) { - joint->limot2.set (parameter & 0xff,value); - } - else { - if (parameter == dParamSuspensionERP) joint->susp_erp = value; - else if (parameter == dParamSuspensionCFM) joint->susp_cfm = value; - else joint->limot1.set (parameter,value); - } -} - - -extern "C" void dJointGetHinge2Anchor (dxJointHinge2 *joint, dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); - getAnchor (joint,result,joint->anchor1); -} - - -extern "C" void dJointGetHinge2Axis1 (dxJointHinge2 *joint, dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); - if (joint->node[0].body) { - dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis1); - } -} - - -extern "C" void dJointGetHinge2Axis2 (dxJointHinge2 *joint, dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); - if (joint->node[1].body) { - dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis2); - } -} - - -extern "C" dReal dJointGetHinge2Param (dxJointHinge2 *joint, int parameter) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); - if ((parameter & 0xff00) == 0x100) { - return joint->limot2.get (parameter & 0xff); - } - else { - if (parameter == dParamSuspensionERP) return joint->susp_erp; - else if (parameter == dParamSuspensionCFM) return joint->susp_cfm; - else return joint->limot1.get (parameter); - } -} - - -extern "C" dReal dJointGetHinge2Angle1 (dxJointHinge2 *joint) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); - if (joint->node[0].body) return measureHinge2Angle (joint); - else return 0; -} - - -extern "C" dReal dJointGetHinge2Angle1Rate (dxJointHinge2 *joint) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); - if (joint->node[0].body) { - dVector3 axis; - dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1); - dReal rate = dDOT(axis,joint->node[0].body->avel); - if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); - return rate; - } - else return 0; -} - - -extern "C" dReal dJointGetHinge2Angle2Rate (dxJointHinge2 *joint) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); - if (joint->node[0].body && joint->node[1].body) { - dVector3 axis; - dMULTIPLY0_331 (axis,joint->node[1].body->R,joint->axis2); - dReal rate = dDOT(axis,joint->node[0].body->avel); - if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); - return rate; - } - else return 0; -} - - -dxJoint::Vtable __dhinge2_vtable = { - sizeof(dxJointHinge2), - (dxJoint::init_fn*) hinge2Init, - (dxJoint::getInfo1_fn*) hinge2GetInfo1, - (dxJoint::getInfo2_fn*) hinge2GetInfo2, - dJointTypeHinge2}; - -//**************************************************************************** -// universal - -static void universalInit (dxJointUniversal *j) -{ - dSetZero (j->anchor1,4); - dSetZero (j->anchor2,4); - dSetZero (j->axis1,4); - j->axis1[0] = 1; - dSetZero (j->axis2,4); - j->axis2[1] = 1; -} - - -static void universalGetInfo1 (dxJointUniversal *j, dxJoint::Info1 *info) -{ - info->nub = 4; - info->m = 4; -} - - -static void universalGetInfo2 (dxJointUniversal *joint, dxJoint::Info2 *info) -{ - // set the three ball-and-socket rows - setBall (joint,info,joint->anchor1,joint->anchor2); - - // set the universal joint row. the angular velocity about an axis - // perpendicular to both joint axes should be equal. thus the constraint - // equation is - // p*w1 - p*w2 = 0 - // where p is a vector normal to both joint axes, and w1 and w2 - // are the angular velocity vectors of the two bodies. - - // length 1 joint axis in global coordinates, from each body - dVector3 ax1, ax2; - // length 1 vector perpendicular to ax1 and ax2. Neither body can rotate - // about this. - dVector3 p; - - // This says "ax1 = joint->node[0].body->R * joint->axis1" - dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); - if (joint->node[1].body) { - dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); - } - else { - ax2[0] = joint->axis2[0]; - ax2[1] = joint->axis2[1]; - ax2[2] = joint->axis2[2]; - } - - // if ax1 and ax2 are almost parallel, p won't be perpendicular to them. - // Is there some more robust way to do this? - dCROSS(p, =, ax1, ax2); - dNormalize3(p); - - int s3=3*info->rowskip; - - info->J1a[s3+0] = p[0]; - info->J1a[s3+1] = p[1]; - info->J1a[s3+2] = p[2]; - - if (joint->node[1].body) { - info->J2a[s3+0] = -p[0]; - info->J2a[s3+1] = -p[1]; - info->J2a[s3+2] = -p[2]; - } - - // compute the right hand side of the constraint equation. set relative - // body velocities along p to bring the axes back to perpendicular. - // If ax1, ax2 are unit length joint axes as computed from body1 and - // body2, we need to rotate both bodies along the axis p. If theta - // is the angle between ax1 and ax2, we need an angular velocity - // along p to cover the angle erp * (theta - Pi/2) in one step: - // - // |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize - // = (erp*fps) * (theta - Pi/2) - // - // if theta is close to Pi/2, - // theta - Pi/2 ~= cos(theta), so - // |angular_velocity| = (erp*fps) * (ax1 dot ax2) - - info->c[3] = info->fps * info->erp * - dDOT(ax1, ax2); -} - - -extern "C" void dJointSetUniversalAnchor (dxJointUniversal *joint, - dReal x, dReal y, dReal z) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); -} - - -extern "C" void dJointSetUniversalAxis1 (dxJointUniversal *joint, - dReal x, dReal y, dReal z) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - if (joint->node[0].body) { - dReal q[4]; - q[0] = x; - q[1] = y; - q[2] = z; - q[3] = 0; - dNormalize3 (q); - dMULTIPLY1_331 (joint->axis1,joint->node[0].body->R,q); - } - joint->axis1[3] = 0; -} - - -extern "C" void dJointSetUniversalAxis2 (dxJointUniversal *joint, - dReal x, dReal y, dReal z) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - if (joint->node[1].body) { - dReal q[4]; - q[0] = x; - q[1] = y; - q[2] = z; - q[3] = 0; - dNormalize3 (q); - dMULTIPLY1_331 (joint->axis2,joint->node[1].body->R,q); - } - joint->axis2[3] = 0; -} - - -extern "C" void dJointGetUniversalAnchor (dxJointUniversal *joint, - dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - getAnchor (joint,result,joint->anchor1); -} - - -extern "C" void dJointGetUniversalAxis1 (dxJointUniversal *joint, - dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - if (joint->node[0].body) { - dMULTIPLY0_331 (result, joint->node[0].body->R, joint->axis1); - } -} - - -extern "C" void dJointGetUniversalAxis2 (dxJointUniversal *joint, - dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - if (joint->node[1].body) { - dMULTIPLY0_331 (result, joint->node[1].body->R, joint->axis2); - } -} - - -dxJoint::Vtable __duniversal_vtable = { - sizeof(dxJointUniversal), - (dxJoint::init_fn*) universalInit, - (dxJoint::getInfo1_fn*) universalGetInfo1, - (dxJoint::getInfo2_fn*) universalGetInfo2, - dJointTypeUniversal}; - -//**************************************************************************** -// angular motor - -static void amotorInit (dxJointAMotor *j) -{ - int i; - j->num = 0; - j->mode = dAMotorUser; - for (i=0; i<3; i++) { - j->rel[i] = 0; - dSetZero (j->axis[i],4); - j->limot[i].init (j->world); - j->angle[i] = 0; - } - dSetZero (j->reference1,4); - dSetZero (j->reference2,4); - - j->flags |= dJOINT_TWOBODIES; -} - - -// compute the 3 axes in global coordinates - -static void amotorComputeGlobalAxes (dxJointAMotor *joint, dVector3 ax[3]) -{ - if (joint->mode == dAMotorEuler) { - // special handling for euler mode - dMULTIPLY0_331 (ax[0],joint->node[0].body->R,joint->axis[0]); - dMULTIPLY0_331 (ax[2],joint->node[1].body->R,joint->axis[2]); - dCROSS (ax[1],=,ax[2],ax[0]); - dNormalize3 (ax[1]); - } - else { - for (int i=0; i < joint->num; i++) { - if (joint->rel[i] == 1) { - // relative to b1 - dMULTIPLY0_331 (ax[i],joint->node[0].body->R,joint->axis[i]); - } - if (joint->rel[i] == 2) { - // relative to b2 - dMULTIPLY0_331 (ax[i],joint->node[1].body->R,joint->axis[i]); - } - else { - // global - just copy it - ax[i][0] = joint->axis[i][0]; - ax[i][1] = joint->axis[i][1]; - ax[i][2] = joint->axis[i][2]; - } - } - } -} - - -static void amotorComputeEulerAngles (dxJointAMotor *joint, dVector3 ax[3]) -{ - // assumptions: - // global axes already calculated --> ax - // axis[0] is relative to body 1 --> global ax[0] - // axis[2] is relative to body 2 --> global ax[2] - // ax[1] = ax[2] x ax[0] - // original ax[0] and ax[2] are perpendicular - // reference1 is perpendicular to ax[0] (in body 1 frame) - // reference2 is perpendicular to ax[2] (in body 2 frame) - // all ax[] and reference vectors are unit length - - // calculate references in global frame - dVector3 ref1,ref2; - dMULTIPLY0_331 (ref1,joint->node[0].body->R,joint->reference1); - dMULTIPLY0_331 (ref2,joint->node[1].body->R,joint->reference2); - - // get q perpendicular to both ax[0] and ref1, get first euler angle - dVector3 q; - dCROSS (q,=,ax[0],ref1); - joint->angle[0] = -dAtan2 (dDOT(ax[2],q),dDOT(ax[2],ref1)); - - // get q perpendicular to both ax[0] and ax[1], get second euler angle - dCROSS (q,=,ax[0],ax[1]); - joint->angle[1] = -dAtan2 (dDOT(ax[2],ax[0]),dDOT(ax[2],q)); - - // get q perpendicular to both ax[1] and ax[2], get third euler angle - dCROSS (q,=,ax[1],ax[2]); - joint->angle[2] = -dAtan2 (dDOT(ref2,ax[1]), dDOT(ref2,q)); -} - - -// set the reference vectors as follows: -// * reference1 = current axis[2] relative to body 1 -// * reference2 = current axis[0] relative to body 2 -// this assumes that: -// * axis[0] is relative to body 1 -// * axis[2] is relative to body 2 - -static void amotorSetEulerReferenceVectors (dxJointAMotor *j) -{ - if (j->node[0].body && j->node[1].body) { - dVector3 r; // axis[2] and axis[0] in global coordinates - dMULTIPLY0_331 (r,j->node[1].body->R,j->axis[2]); - dMULTIPLY1_331 (j->reference1,j->node[0].body->R,r); - dMULTIPLY0_331 (r,j->node[0].body->R,j->axis[0]); - dMULTIPLY1_331 (j->reference2,j->node[1].body->R,r); - } -} - - -static void amotorGetInfo1 (dxJointAMotor *j, dxJoint::Info1 *info) -{ - info->m = 0; - info->nub = 0; - - // compute the axes and angles, if in euler mode - if (j->mode == dAMotorEuler) { - dVector3 ax[3]; - amotorComputeGlobalAxes (j,ax); - amotorComputeEulerAngles (j,ax); - } - - // see if we're powered or at a joint limit for each axis - for (int i=0; i < j->num; i++) { - if (j->limot[i].testRotationalLimit (j->angle[i]) || - j->limot[i].fmax > 0) { - info->m++; - } - } -} - - -static void amotorGetInfo2 (dxJointAMotor *joint, dxJoint::Info2 *info) -{ - int i; - - // compute the axes (if not global) - dVector3 ax[3]; - amotorComputeGlobalAxes (joint,ax); - - // in euler angle mode we do not actually constrain the angular velocity - // along the axes axis[0] and axis[2] (although we do use axis[1]) : - // - // to get constrain w2-w1 along ...not - // ------ --------------------- ------ - // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] - // d(angle[1])/dt = 0 ax[1] - // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] - // - // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. - // to prove the result for angle[0], write the expression for angle[0] from - // GetInfo1 then take the derivative. to prove this for angle[2] it is - // easier to take the euler rate expression for d(angle[2])/dt with respect - // to the components of w and set that to 0. - - dVector3 *axptr[3]; - axptr[0] = &ax[0]; - axptr[1] = &ax[1]; - axptr[2] = &ax[2]; - - dVector3 ax0_cross_ax1; - dVector3 ax1_cross_ax2; - if (joint->mode == dAMotorEuler) { - dCROSS (ax0_cross_ax1,=,ax[0],ax[1]); - axptr[2] = &ax0_cross_ax1; - dCROSS (ax1_cross_ax2,=,ax[1],ax[2]); - axptr[0] = &ax1_cross_ax2; - } - - int row=0; - for (i=0; i < joint->num; i++) { - row += joint->limot[i].addLimot (joint,info,row,*(axptr[i]),1); - } -} - - -extern "C" void dJointSetAMotorNumAxes (dxJointAMotor *joint, int num) -{ - dAASSERT(joint && num >= 0 && num <= 3); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - if (joint->mode == dAMotorEuler) { - joint->num = 3; - } - else { - if (num < 0) num = 0; - if (num > 3) num = 3; - joint->num = num; - } -} - - -extern "C" void dJointSetAMotorAxis (dxJointAMotor *joint, int anum, int rel, - dReal x, dReal y, dReal z) -{ - dAASSERT(joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - if (anum < 0) anum = 0; - if (anum > 2) anum = 2; - joint->rel[anum] = rel; - - // x,y,z is always in global coordinates regardless of rel, so we may have - // to convert it to be relative to a body - dVector3 r; - r[0] = x; - r[1] = y; - r[2] = z; - r[3] = 0; - if (rel > 0) { - if (rel==1) { - dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->R,r); - } - else { - dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->R,r); - } - } - else { - joint->axis[anum][0] = r[0]; - joint->axis[anum][1] = r[1]; - joint->axis[anum][2] = r[2]; - } - dNormalize3 (joint->axis[anum]); - if (joint->mode == dAMotorEuler) amotorSetEulerReferenceVectors (joint); -} - - -extern "C" void dJointSetAMotorAngle (dxJointAMotor *joint, int anum, - dReal angle) -{ - dAASSERT(joint && anum >= 0 && anum < 3); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - if (joint->mode == dAMotorUser) { - if (anum < 0) anum = 0; - if (anum > 3) anum = 3; - joint->angle[anum] = angle; - } -} - - -extern "C" void dJointSetAMotorParam (dxJointAMotor *joint, int parameter, - dReal value) -{ - dAASSERT(joint); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - int anum = parameter >> 8; - if (anum < 0) anum = 0; - if (anum > 2) anum = 2; - parameter &= 0xff; - joint->limot[anum].set (parameter, value); -} - - -extern "C" void dJointSetAMotorMode (dxJointAMotor *joint, int mode) -{ - dAASSERT(joint); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - joint->mode = mode; - if (joint->mode == dAMotorEuler) { - joint->num = 3; - amotorSetEulerReferenceVectors (joint); - } -} - - -extern "C" int dJointGetAMotorNumAxes (dxJointAMotor *joint) -{ - dAASSERT(joint); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - return joint->num; -} - - -extern "C" void dJointGetAMotorAxis (dxJointAMotor *joint, int anum, - dVector3 result) -{ - dAASSERT(joint && anum >= 0 && anum < 3); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - if (anum < 0) anum = 0; - if (anum > 2) anum = 2; - if (joint->rel[anum] > 0) { - if (joint->rel[anum]==1) { - dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis[anum]); - } - else { - dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis[anum]); - } - } - else { - result[0] = joint->axis[anum][0]; - result[1] = joint->axis[anum][1]; - result[2] = joint->axis[anum][2]; - } -} - - -extern "C" int dJointGetAMotorAxisRel (dxJointAMotor *joint, int anum) -{ - dAASSERT(joint && anum >= 0 && anum < 3); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - if (anum < 0) anum = 0; - if (anum > 2) anum = 2; - return joint->rel[anum]; -} - - -extern "C" dReal dJointGetAMotorAngle (dxJointAMotor *joint, int anum) -{ - dAASSERT(joint && anum >= 0 && anum < 3); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - if (anum < 0) anum = 0; - if (anum > 3) anum = 3; - return joint->angle[anum]; -} - - -extern "C" dReal dJointGetAMotorAngleRate (dxJointAMotor *joint, int anum) -{ - // @@@ - dDebug (0,"not yet implemented"); - return 0; -} - - -extern "C" dReal dJointGetAMotorParam (dxJointAMotor *joint, int parameter) -{ - dAASSERT(joint); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - int anum = parameter >> 8; - if (anum < 0) anum = 0; - if (anum > 2) anum = 2; - parameter &= 0xff; - return joint->limot[anum].get (parameter); -} - - -extern "C" int dJointGetAMotorMode (dxJointAMotor *joint) -{ - dAASSERT(joint); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - return joint->mode; -} - - -dxJoint::Vtable __damotor_vtable = { - sizeof(dxJointAMotor), - (dxJoint::init_fn*) amotorInit, - (dxJoint::getInfo1_fn*) amotorGetInfo1, - (dxJoint::getInfo2_fn*) amotorGetInfo2, - dJointTypeAMotor}; - -//**************************************************************************** -// fixed joint - -static void fixedInit (dxJointFixed *j) -{ - dSetZero (j->offset,4); -} - - -static void fixedGetInfo1 (dxJointFixed *j, dxJoint::Info1 *info) -{ - info->m = 6; - info->nub = 6; -} - - -static void fixedGetInfo2 (dxJointFixed *joint, dxJoint::Info2 *info) -{ - int s = info->rowskip; - - // set jacobian - info->J1l[0] = 1; - info->J1l[s+1] = 1; - info->J1l[2*s+2] = 1; - info->J1a[3*s] = 1; - info->J1a[4*s+1] = 1; - info->J1a[5*s+2] = 1; - - dVector3 ofs; - if (joint->node[1].body) { - dMULTIPLY0_331 (ofs,joint->node[0].body->R,joint->offset); - dCROSSMAT (info->J1a,ofs,s,+,-); - info->J2l[0] = -1; - info->J2l[s+1] = -1; - info->J2l[2*s+2] = -1; - info->J2a[3*s] = -1; - info->J2a[4*s+1] = -1; - info->J2a[5*s+2] = -1; - } - - // set right hand side for the first three rows (linear) - dReal k = info->fps * info->erp; - if (joint->node[1].body) { - for (int j=0; j<3; j++) - info->c[j] = k * (joint->node[1].body->pos[j] - - joint->node[0].body->pos[j] + ofs[j]); - } - else { - for (int j=0; j<3; j++) - info->c[j] = k * (joint->offset[j] - joint->node[0].body->pos[j]); - } - - // set right hand side for the next three rows (angular). this code is - // borrowed from the slider, so look at the comments there. - // @@@ make a function common to both the slider and this joint !!! - - // get qerr = relative rotation (rotation error) between two bodies - dQuaternion qerr,e; - if (joint->node[1].body) { - dQMultiply1 (qerr,joint->node[0].body->q,joint->node[1].body->q); - } - else { - qerr[0] = joint->node[0].body->q[0]; - for (int i=1; i<4; i++) qerr[i] = -joint->node[0].body->q[i]; - } - if (qerr[0] < 0) { - qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small - qerr[2] = -qerr[2]; - qerr[3] = -qerr[3]; - } - dMULTIPLY0_331 (e,joint->node[0].body->R,qerr+1); // @@@ bad SIMD padding! - info->c[3] = 2*k * e[0]; - info->c[4] = 2*k * e[1]; - info->c[5] = 2*k * e[2]; -} - - -extern "C" void dJointSetFixed (dxJointFixed *joint) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not fixed"); - int i; - - // compute the offset between the bodies - if (joint->node[0].body) { - if (joint->node[1].body) { - dReal ofs[4]; - for (i=0; i<4; i++) ofs[i] = joint->node[0].body->pos[i]; - for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->pos[i]; - dMULTIPLY1_331 (joint->offset,joint->node[0].body->R,ofs); - } - else { - for (i=0; i<4; i++) joint->offset[i] = joint->node[0].body->pos[i]; - } - } -} - - -dxJoint::Vtable __dfixed_vtable = { - sizeof(dxJointFixed), - (dxJoint::init_fn*) fixedInit, - (dxJoint::getInfo1_fn*) fixedGetInfo1, - (dxJoint::getInfo2_fn*) fixedGetInfo2, - dJointTypeFixed}; - -//**************************************************************************** -// null joint - -static void nullGetInfo1 (dxJointNull *j, dxJoint::Info1 *info) -{ - info->m = 0; - info->nub = 0; -} - - -static void nullGetInfo2 (dxJointNull *joint, dxJoint::Info2 *info) -{ - dDebug (0,"this should never get called"); -} - - -dxJoint::Vtable __dnull_vtable = { - sizeof(dxJointNull), - (dxJoint::init_fn*) 0, - (dxJoint::getInfo1_fn*) nullGetInfo1, - (dxJoint::getInfo2_fn*) nullGetInfo2, - dJointTypeNull}; diff --git a/extern/ode/dist/ode/src/joint.h b/extern/ode/dist/ode/src/joint.h deleted file mode 100644 index e0362ffa829..00000000000 --- a/extern/ode/dist/ode/src/joint.h +++ /dev/null @@ -1,260 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#ifndef _ODE_JOINT_H_ -#define _ODE_JOINT_H_ - - -#include "objects.h" -#include <ode/contact.h> -#include "obstack.h" - - -// joint flags -enum { - // if this flag is set, the joint was allocated in a joint group - dJOINT_INGROUP = 1, - - // if this flag is set, the joint was attached with arguments (0,body). - // our convention is to treat all attaches as (body,0), i.e. so node[0].body - // is always nonzero, so this flag records the fact that the arguments were - // swapped. - dJOINT_REVERSE = 2, - - // if this flag is set, the joint can not have just one body attached to it, - // it must have either zero or two bodies attached. - dJOINT_TWOBODIES = 4 -}; - - -// there are two of these nodes in the joint, one for each connection to a -// body. these are node of a linked list kept by each body of it's connecting -// joints. but note that the body pointer in each node points to the body that -// makes use of the *other* node, not this node. this trick makes it a bit -// easier to traverse the body/joint graph. - -struct dxJointNode { - dxJoint *joint; // pointer to enclosing dxJoint object - dxBody *body; // *other* body this joint is connected to - dxJointNode *next; // next node in body's list of connected joints -}; - - -struct dxJoint : public dObject { - // naming convention: the "first" body this is connected to is node[0].body, - // and the "second" body is node[1].body. if this joint is only connected - // to one body then the second body is 0. - - // info returned by getInfo1 function. the constraint dimension is m (<=6). - // i.e. that is the total number of rows in the jacobian. `nub' is the - // number of unbounded variables (which have lo,hi = -/+ infinity). - - struct Info1 { - int m,nub; - }; - - // info returned by getInfo2 function - - struct Info2 { - // integrator parameters: frames per second (1/stepsize), default error - // reduction parameter (0..1). - dReal fps,erp; - - // for the first and second body, pointers to two (linear and angular) - // n*3 jacobian sub matrices, stored by rows. these matrices will have - // been initialized to 0 on entry. if the second body is zero then the - // J2xx pointers may be 0. - dReal *J1l,*J1a,*J2l,*J2a; - - // elements to jump from one row to the next in J's - int rowskip; - - // right hand sides of the equation J*v = c + cfm * lambda. cfm is the - // "constraint force mixing" vector. c is set to zero on entry, cfm is - // set to a constant value (typically very small or zero) value on entry. - dReal *c,*cfm; - - // lo and hi limits for variables (set to -/+ infinity on entry). - dReal *lo,*hi; - - // findex vector for variables. see the LCP solver interface for a - // description of what this does. this is set to -1 on entry. - // note that the returned indexes are relative to the first index of - // the constraint. - int *findex; - }; - - // virtual function table: size of the joint structure, function pointers. - // we do it this way instead of using C++ virtual functions because - // sometimes we need to allocate joints ourself within a memory pool. - - typedef void init_fn (dxJoint *joint); - typedef void getInfo1_fn (dxJoint *joint, Info1 *info); - typedef void getInfo2_fn (dxJoint *joint, Info2 *info); - struct Vtable { - int size; - init_fn *init; - getInfo1_fn *getInfo1; - getInfo2_fn *getInfo2; - int typenum; // a dJointTypeXXX type number - }; - - Vtable *vtable; // virtual function table - int flags; // dJOINT_xxx flags - dxJointNode node[2]; // connections to bodies. node[1].body can be 0 - dJointFeedback *feedback; // optional feedback structure -}; - - -// joint group. NOTE: any joints in the group that have their world destroyed -// will have their world pointer set to 0. - -struct dxJointGroup : public dBase { - int num; // number of joints on the stack - dObStack stack; // a stack of (possibly differently sized) dxJoint -}; // objects. - - -// common limit and motor information for a single joint axis of movement -struct dxJointLimitMotor { - dReal vel,fmax; // powered joint: velocity, max force - dReal lostop,histop; // joint limits, relative to initial position - dReal fudge_factor; // when powering away from joint limits - dReal normal_cfm; // cfm to use when not at a stop - dReal stop_erp,stop_cfm; // erp and cfm for when at joint limit - dReal bounce; // restitution factor - // variables used between getInfo1() and getInfo2() - int limit; // 0=free, 1=at lo limit, 2=at hi limit - dReal limit_err; // if at limit, amount over limit - - void init (dxWorld *); - void set (int num, dReal value); - dReal get (int num); - int testRotationalLimit (dReal angle); - int addLimot (dxJoint *joint, dxJoint::Info2 *info, int row, - dVector3 ax1, int rotational); -}; - - -// ball and socket - -struct dxJointBall : public dxJoint { - dVector3 anchor1; // anchor w.r.t first body - dVector3 anchor2; // anchor w.r.t second body -}; -extern struct dxJoint::Vtable __dball_vtable; - - -// hinge - -struct dxJointHinge : public dxJoint { - dVector3 anchor1; // anchor w.r.t first body - dVector3 anchor2; // anchor w.r.t second body - dVector3 axis1; // axis w.r.t first body - dVector3 axis2; // axis w.r.t second body - dQuaternion qrel; // initial relative rotation body1 -> body2 - dxJointLimitMotor limot; // limit and motor information -}; -extern struct dxJoint::Vtable __dhinge_vtable; - - -// universal - -struct dxJointUniversal : public dxJoint { - dVector3 anchor1; // anchor w.r.t first body - dVector3 anchor2; // anchor w.r.t second body - dVector3 axis1; // axis w.r.t first body - dVector3 axis2; // axis w.r.t second body -}; -extern struct dxJoint::Vtable __duniversal_vtable; - - -// slider. if body2 is 0 then qrel is the absolute rotation of body1 and -// offset is the position of body1 center along axis1. - -struct dxJointSlider : public dxJoint { - dVector3 axis1; // axis w.r.t first body - dQuaternion qrel; // initial relative rotation body1 -> body2 - dVector3 offset; // point relative to body2 that should be - // aligned with body1 center along axis1 - dxJointLimitMotor limot; // limit and motor information -}; -extern struct dxJoint::Vtable __dslider_vtable; - - -// contact - -struct dxJointContact : public dxJoint { - int the_m; // number of rows computed by getInfo1 - dContact contact; -}; -extern struct dxJoint::Vtable __dcontact_vtable; - - -// hinge 2 - -struct dxJointHinge2 : public dxJoint { - dVector3 anchor1; // anchor w.r.t first body - dVector3 anchor2; // anchor w.r.t second body - dVector3 axis1; // axis 1 w.r.t first body - dVector3 axis2; // axis 2 w.r.t second body - dReal c0,s0; // cos,sin of desired angle between axis 1,2 - dVector3 v1,v2; // angle ref vectors embedded in first body - dxJointLimitMotor limot1; // limit+motor info for axis 1 - dxJointLimitMotor limot2; // limit+motor info for axis 2 - dReal susp_erp,susp_cfm; // suspension parameters (erp,cfm) -}; -extern struct dxJoint::Vtable __dhinge2_vtable; - - -// angular motor - -struct dxJointAMotor : public dxJoint { - int num; // number of axes (0..3) - int mode; // a dAMotorXXX constant - int rel[3]; // what the axes are relative to (global,b1,b2) - dVector3 axis[3]; // three axes - dxJointLimitMotor limot[3]; // limit+motor info for axes - dReal angle[3]; // user-supplied angles for axes - // these vectors are used for calculating euler angles - dVector3 reference1; // original axis[2], relative to body 1 - dVector3 reference2; // original axis[0], relative to body 2 -}; -extern struct dxJoint::Vtable __damotor_vtable; - - -// fixed - -struct dxJointFixed : public dxJoint { - dVector3 offset; // relative offset between the bodies -}; -extern struct dxJoint::Vtable __dfixed_vtable; - - -// null joint, for testing only - -struct dxJointNull : public dxJoint { -}; -extern struct dxJoint::Vtable __dnull_vtable; - -#endif - diff --git a/extern/ode/dist/ode/src/lcp.cpp b/extern/ode/dist/ode/src/lcp.cpp deleted file mode 100644 index dba2d3b949b..00000000000 --- a/extern/ode/dist/ode/src/lcp.cpp +++ /dev/null @@ -1,1455 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -/* - - -THE ALGORITHM -------------- - -solve A*x = b+w, with x and w subject to certain LCP conditions. -each x(i),w(i) must lie on one of the three line segments in the following -diagram. each line segment corresponds to one index set : - - w(i) - /|\ | : - | | : - | |i in N : - w>0 | |state[i]=0 : - | | : - | | : i in C - w=0 + +-----------------------+ - | : | - | : | - w<0 | : |i in N - | : |state[i]=1 - | : | - | : | - +-------|-----------|-----------|----------> x(i) - lo 0 hi - -the Dantzig algorithm proceeds as follows: - for i=1:n - * if (x(i),w(i)) is not on the line, push x(i) and w(i) positive or - negative towards the line. as this is done, the other (x(j),w(j)) - for j<i are constrained to be on the line. if any (x,w) reaches the - end of a line segment then it is switched between index sets. - * i is added to the appropriate index set depending on what line segment - it hits. - -we restrict lo(i) <= 0 and hi(i) >= 0. this makes the algorithm a bit -simpler, because the starting point for x(i),w(i) is always on the dotted -line x=0 and x will only ever increase in one direction, so it can only hit -two out of the three line segments. - - -NOTES ------ - -this is an implementation of "lcp_dantzig2_ldlt.m" and "lcp_dantzig_lohi.m". -the implementation is split into an LCP problem object (dLCP) and an LCP -driver function. most optimization occurs in the dLCP object. - -a naive implementation of the algorithm requires either a lot of data motion -or a lot of permutation-array lookup, because we are constantly re-ordering -rows and columns. to avoid this and make a more optimized algorithm, a -non-trivial data structure is used to represent the matrix A (this is -implemented in the fast version of the dLCP object). - -during execution of this algorithm, some indexes in A are clamped (set C), -some are non-clamped (set N), and some are "don't care" (where x=0). -A,x,b,w (and other problem vectors) are permuted such that the clamped -indexes are first, the unclamped indexes are next, and the don't-care -indexes are last. this permutation is recorded in the array `p'. -initially p = 0..n-1, and as the rows and columns of A,x,b,w are swapped, -the corresponding elements of p are swapped. - -because the C and N elements are grouped together in the rows of A, we can do -lots of work with a fast dot product function. if A,x,etc were not permuted -and we only had a permutation array, then those dot products would be much -slower as we would have a permutation array lookup in some inner loops. - -A is accessed through an array of row pointers, so that element (i,j) of the -permuted matrix is A[i][j]. this makes row swapping fast. for column swapping -we still have to actually move the data. - -during execution of this algorithm we maintain an L*D*L' factorization of -the clamped submatrix of A (call it `AC') which is the top left nC*nC -submatrix of A. there are two ways we could arrange the rows/columns in AC. - -(1) AC is always permuted such that L*D*L' = AC. this causes a problem - when a row/column is removed from C, because then all the rows/columns of A - between the deleted index and the end of C need to be rotated downward. - this results in a lot of data motion and slows things down. -(2) L*D*L' is actually a factorization of a *permutation* of AC (which is - itself a permutation of the underlying A). this is what we do - the - permutation is recorded in the vector C. call this permutation A[C,C]. - when a row/column is removed from C, all we have to do is swap two - rows/columns and manipulate C. - -*/ - -#include <ode/common.h> -#include "lcp.h" -#include <ode/matrix.h> -#include <ode/misc.h> -#include "mat.h" // for testing -#include <ode/timer.h> // for testing - -//*************************************************************************** -// code generation parameters - -// LCP debugging (mosty for fast dLCP) - this slows things down a lot -//#define DEBUG_LCP - -//#define dLCP_SLOW // use slow dLCP object -#define dLCP_FAST // use fast dLCP object - -// option 1 : matrix row pointers (less data copying) -#define ROWPTRS -#define ATYPE dReal ** -#define AROW(i) (A[i]) - -// option 2 : no matrix row pointers (slightly faster inner loops) -//#define NOROWPTRS -//#define ATYPE dReal * -//#define AROW(i) (A+(i)*nskip) - -// misc defines -#define ALLOCA dALLOCA16 -//#define dDot myDot -#define NUB_OPTIMIZATIONS - -//*************************************************************************** - -// an alternative inline dot product, for speed comparisons - -static inline dReal myDot (dReal *a, dReal *b, int n) -{ - dReal sum=0; - while (n > 0) { - sum += (*a) * (*b); - a++; - b++; - n--; - } - return sum; -} - - -// swap row/column i1 with i2 in the n*n matrix A. the leading dimension of -// A is nskip. this only references and swaps the lower triangle. -// if `do_fast_row_swaps' is nonzero and row pointers are being used, then -// rows will be swapped by exchanging row pointers. otherwise the data will -// be copied. - -static void swapRowsAndCols (ATYPE A, int n, int i1, int i2, int nskip, - int do_fast_row_swaps) -{ - int i; - dIASSERT (A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n && - nskip >= n && i1 < i2); - -# ifdef ROWPTRS - for (i=i1+1; i<i2; i++) A[i1][i] = A[i][i1]; - for (i=i1+1; i<i2; i++) A[i][i1] = A[i2][i]; - A[i1][i2] = A[i1][i1]; - A[i1][i1] = A[i2][i1]; - A[i2][i1] = A[i2][i2]; - // swap rows, by swapping row pointers - if (do_fast_row_swaps) { - dReal *tmpp; - tmpp = A[i1]; - A[i1] = A[i2]; - A[i2] = tmpp; - } - else { - dReal *tmprow = (dReal*) ALLOCA (n * sizeof(dReal)); - memcpy (tmprow,A[i1],n * sizeof(dReal)); - memcpy (A[i1],A[i2],n * sizeof(dReal)); - memcpy (A[i2],tmprow,n * sizeof(dReal)); - } - // swap columns the hard way - for (i=i2+1; i<n; i++) { - dReal tmp = A[i][i1]; - A[i][i1] = A[i][i2]; - A[i][i2] = tmp; - } -# else - dReal tmp,*tmprow = (dReal*) ALLOCA (n * sizeof(dReal)); - if (i1 > 0) { - memcpy (tmprow,A+i1*nskip,i1*sizeof(dReal)); - memcpy (A+i1*nskip,A+i2*nskip,i1*sizeof(dReal)); - memcpy (A+i2*nskip,tmprow,i1*sizeof(dReal)); - } - for (i=i1+1; i<i2; i++) { - tmp = A[i2*nskip+i]; - A[i2*nskip+i] = A[i*nskip+i1]; - A[i*nskip+i1] = tmp; - } - tmp = A[i1*nskip+i1]; - A[i1*nskip+i1] = A[i2*nskip+i2]; - A[i2*nskip+i2] = tmp; - for (i=i2+1; i<n; i++) { - tmp = A[i*nskip+i1]; - A[i*nskip+i1] = A[i*nskip+i2]; - A[i*nskip+i2] = tmp; - } -# endif -} - - -// swap two indexes in the n*n LCP problem. i1 must be <= i2. - -static void swapProblem (ATYPE A, dReal *x, dReal *b, dReal *w, dReal *lo, - dReal *hi, int *p, int *state, int *findex, - int n, int i1, int i2, int nskip, - int do_fast_row_swaps) -{ - dReal tmp; - int tmpi; - dIASSERT (n>0 && i1 >=0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n && - i1 <= i2); - if (i1==i2) return; - swapRowsAndCols (A,n,i1,i2,nskip,do_fast_row_swaps); - tmp = x[i1]; - x[i1] = x[i2]; - x[i2] = tmp; - tmp = b[i1]; - b[i1] = b[i2]; - b[i2] = tmp; - tmp = w[i1]; - w[i1] = w[i2]; - w[i2] = tmp; - tmp = lo[i1]; - lo[i1] = lo[i2]; - lo[i2] = tmp; - tmp = hi[i1]; - hi[i1] = hi[i2]; - hi[i2] = tmp; - tmpi = p[i1]; - p[i1] = p[i2]; - p[i2] = tmpi; - tmpi = state[i1]; - state[i1] = state[i2]; - state[i2] = tmpi; - if (findex) { - tmpi = findex[i1]; - findex[i1] = findex[i2]; - findex[i2] = tmpi; - } -} - - -// for debugging - check that L,d is the factorization of A[C,C]. -// A[C,C] has size nC*nC and leading dimension nskip. -// L has size nC*nC and leading dimension nskip. -// d has size nC. - -#ifdef DEBUG_LCP - -static void checkFactorization (ATYPE A, dReal *_L, dReal *_d, - int nC, int *C, int nskip) -{ - int i,j; - if (nC==0) return; - - // get A1=A, copy the lower triangle to the upper triangle, get A2=A[C,C] - dMatrix A1 (nC,nC); - for (i=0; i<nC; i++) { - for (j=0; j<=i; j++) A1(i,j) = A1(j,i) = AROW(i)[j]; - } - dMatrix A2 = A1.select (nC,C,nC,C); - - // printf ("A1=\n"); A1.print(); printf ("\n"); - // printf ("A2=\n"); A2.print(); printf ("\n"); - - // compute A3 = L*D*L' - dMatrix L (nC,nC,_L,nskip,1); - dMatrix D (nC,nC); - for (i=0; i<nC; i++) D(i,i) = 1/_d[i]; - L.clearUpperTriangle(); - for (i=0; i<nC; i++) L(i,i) = 1; - dMatrix A3 = L * D * L.transpose(); - - // printf ("L=\n"); L.print(); printf ("\n"); - // printf ("D=\n"); D.print(); printf ("\n"); - // printf ("A3=\n"); A2.print(); printf ("\n"); - - // compare A2 and A3 - dReal diff = A2.maxDifference (A3); - if (diff > 1e-8) - dDebug (0,"L*D*L' check, maximum difference = %.6e\n",diff); -} - -#endif - - -// for debugging - -#ifdef DEBUG_LCP - -static void checkPermutations (int i, int n, int nC, int nN, int *p, int *C) -{ - int j,k; - dIASSERT (nC>=0 && nN>=0 && (nC+nN)==i && i < n); - for (k=0; k<i; k++) dIASSERT (p[k] >= 0 && p[k] < i); - for (k=i; k<n; k++) dIASSERT (p[k] == k); - for (j=0; j<nC; j++) { - int C_is_bad = 1; - for (k=0; k<nC; k++) if (C[k]==j) C_is_bad = 0; - dIASSERT (C_is_bad==0); - } -} - -#endif - -//*************************************************************************** -// dLCP manipulator object. this represents an n*n LCP problem. -// -// two index sets C and N are kept. each set holds a subset of -// the variable indexes 0..n-1. an index can only be in one set. -// initially both sets are empty. -// -// the index set C is special: solutions to A(C,C)\A(C,i) can be generated. - -#ifdef dLCP_SLOW - -// simple but slow implementation of dLCP, for testing the LCP drivers. - -#include "array.h" - -struct dLCP { - int n,nub,nskip; - dReal *Adata,*x,*b,*w,*lo,*hi; // LCP problem data - ATYPE A; // A rows - dArray<int> C,N; // index sets - int last_i_for_solve1; // last i value given to solve1 - - dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w, - dReal *_lo, dReal *_hi, dReal *_L, dReal *_d, - dReal *_Dell, dReal *_ell, dReal *_tmp, - int *_state, int *_findex, int *_p, int *_C, dReal **Arows); - // the constructor is given an initial problem description (A,x,b,w) and - // space for other working data (which the caller may allocate on the stack). - // some of this data is specific to the fast dLCP implementation. - // the matrices A and L have size n*n, vectors have size n*1. - // A represents a symmetric matrix but only the lower triangle is valid. - // `nub' is the number of unbounded indexes at the start. all the indexes - // 0..nub-1 will be put into C. - - ~dLCP(); - - int getNub() { return nub; } - // return the value of `nub'. the constructor may want to change it, - // so the caller should find out its new value. - - // transfer functions: transfer index i to the given set (C or N). indexes - // less than `nub' can never be given. A,x,b,w,etc may be permuted by these - // functions, the caller must be robust to this. - - void transfer_i_to_C (int i); - // this assumes C and N span 1:i-1. this also assumes that solve1() has - // been recently called for the same i without any other transfer - // functions in between (thereby allowing some data reuse for the fast - // implementation). - void transfer_i_to_N (int i); - // this assumes C and N span 1:i-1. - void transfer_i_from_N_to_C (int i); - void transfer_i_from_C_to_N (int i); - - int numC(); - int numN(); - // return the number of indexes in set C/N - - int indexC (int i); - int indexN (int i); - // return index i in set C/N. - - // accessor and arithmetic functions. Aij translates as A(i,j), etc. - // make sure that only the lower triangle of A is ever referenced. - - dReal Aii (int i); - dReal AiC_times_qC (int i, dReal *q); - dReal AiN_times_qN (int i, dReal *q); // for all Nj - void pN_equals_ANC_times_qC (dReal *p, dReal *q); // for all Nj - void pN_plusequals_ANi (dReal *p, int i, int sign=1); - // for all Nj. sign = +1,-1. assumes i > maximum index in N. - void pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q); - void pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q); // for all Nj - void solve1 (dReal *a, int i, int dir=1, int only_transfer=0); - // get a(C) = - dir * A(C,C) \ A(C,i). dir must be +/- 1. - // the fast version of this function computes some data that is needed by - // transfer_i_to_C(). if only_transfer is nonzero then this function - // *only* computes that data, it does not set a(C). - - void unpermute(); - // call this at the end of the LCP function. if the x/w values have been - // permuted then this will unscramble them. -}; - - -dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w, - dReal *_lo, dReal *_hi, dReal *_L, dReal *_d, - dReal *_Dell, dReal *_ell, dReal *_tmp, - int *_state, int *_findex, int *_p, int *_C, dReal **Arows) -{ - dUASSERT (_findex==0,"slow dLCP object does not support findex array"); - - n = _n; - nub = _nub; - Adata = _Adata; - A = 0; - x = _x; - b = _b; - w = _w; - lo = _lo; - hi = _hi; - nskip = dPAD(n); - dSetZero (x,n); - last_i_for_solve1 = -1; - - int i,j; - C.setSize (n); - N.setSize (n); - for (int i=0; i<n; i++) { - C[i] = 0; - N[i] = 0; - } - -# ifdef ROWPTRS - // make matrix row pointers - A = Arows; - for (i=0; i<n; i++) A[i] = Adata + i*nskip; -# else - A = Adata; -# endif - - // lets make A symmetric - for (i=0; i<n; i++) { - for (j=i+1; j<n; j++) AROW(i)[j] = AROW(j)[i]; - } - - // if nub>0, put all indexes 0..nub-1 into C and solve for x - if (nub > 0) { - for (i=0; i<nub; i++) memcpy (_L+i*nskip,AROW(i),(i+1)*sizeof(dReal)); - dFactorLDLT (_L,_d,nub,nskip); - memcpy (x,b,nub*sizeof(dReal)); - dSolveLDLT (_L,_d,x,nub,nskip); - dSetZero (_w,nub); - for (i=0; i<nub; i++) C[i] = 1; - } -} - - -dLCP::~dLCP() -{ -} - - -void dLCP::transfer_i_to_C (int i) -{ - if (i < nub) dDebug (0,"bad i"); - if (C[i]) dDebug (0,"i already in C"); - if (N[i]) dDebug (0,"i already in N"); - for (int k=0; k<i; k++) { - if (!(C[k] ^ N[k])) dDebug (0,"assumptions for C and N violated"); - } - for (int k=i; k<n; k++) - if (C[k] || N[k]) dDebug (0,"assumptions for C and N violated"); - if (i != last_i_for_solve1) dDebug (0,"assumptions for i violated"); - last_i_for_solve1 = -1; - C[i] = 1; -} - - -void dLCP::transfer_i_to_N (int i) -{ - if (i < nub) dDebug (0,"bad i"); - if (C[i]) dDebug (0,"i already in C"); - if (N[i]) dDebug (0,"i already in N"); - for (int k=0; k<i; k++) - if (!C[k] && !N[k]) dDebug (0,"assumptions for C and N violated"); - for (int k=i; k<n; k++) - if (C[k] || N[k]) dDebug (0,"assumptions for C and N violated"); - last_i_for_solve1 = -1; - N[i] = 1; -} - - -void dLCP::transfer_i_from_N_to_C (int i) -{ - if (i < nub) dDebug (0,"bad i"); - if (C[i]) dDebug (0,"i already in C"); - if (!N[i]) dDebug (0,"i not in N"); - last_i_for_solve1 = -1; - N[i] = 0; - C[i] = 1; -} - - -void dLCP::transfer_i_from_C_to_N (int i) -{ - if (i < nub) dDebug (0,"bad i"); - if (N[i]) dDebug (0,"i already in N"); - if (!C[i]) dDebug (0,"i not in C"); - last_i_for_solve1 = -1; - C[i] = 0; - N[i] = 1; -} - - -int dLCP::numC() -{ - int i,count=0; - for (i=0; i<n; i++) if (C[i]) count++; - return count; -} - - -int dLCP::numN() -{ - int i,count=0; - for (i=0; i<n; i++) if (N[i]) count++; - return count; -} - - -int dLCP::indexC (int i) -{ - int k,count=0; - for (k=0; k<n; k++) { - if (C[k]) { - if (count==i) return k; - count++; - } - } - dDebug (0,"bad index C (%d)",i); - return 0; -} - - -int dLCP::indexN (int i) -{ - int k,count=0; - for (k=0; k<n; k++) { - if (N[k]) { - if (count==i) return k; - count++; - } - } - dDebug (0,"bad index into N"); - return 0; -} - - -dReal dLCP::Aii (int i) -{ - return AROW(i)[i]; -} - - -dReal dLCP::AiC_times_qC (int i, dReal *q) -{ - dReal sum = 0; - for (int k=0; k<n; k++) if (C[k]) sum += AROW(i)[k] * q[k]; - return sum; -} - - -dReal dLCP::AiN_times_qN (int i, dReal *q) -{ - dReal sum = 0; - for (int k=0; k<n; k++) if (N[k]) sum += AROW(i)[k] * q[k]; - return sum; -} - - -void dLCP::pN_equals_ANC_times_qC (dReal *p, dReal *q) -{ - dReal sum; - for (int ii=0; ii<n; ii++) if (N[ii]) { - sum = 0; - for (int jj=0; jj<n; jj++) if (C[jj]) sum += AROW(ii)[jj] * q[jj]; - p[ii] = sum; - } -} - - -void dLCP::pN_plusequals_ANi (dReal *p, int i, int sign) -{ - int k; - for (k=0; k<n; k++) if (N[k] && k >= i) dDebug (0,"N assumption violated"); - if (sign > 0) { - for (k=0; k<n; k++) if (N[k]) p[k] += AROW(i)[k]; - } - else { - for (k=0; k<n; k++) if (N[k]) p[k] -= AROW(i)[k]; - } -} - - -void dLCP::pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q) -{ - for (int k=0; k<n; k++) if (C[k]) p[k] += s*q[k]; -} - - -void dLCP::pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q) -{ - for (int k=0; k<n; k++) if (N[k]) p[k] += s*q[k]; -} - - -void dLCP::solve1 (dReal *a, int i, int dir, int only_transfer) -{ - dReal *AA = (dReal*) ALLOCA (n*nskip*sizeof(dReal)); - dReal *dd = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *bb = (dReal*) ALLOCA (n*sizeof(dReal)); - int ii,jj,AAi,AAj; - - last_i_for_solve1 = i; - AAi = 0; - for (ii=0; ii<n; ii++) if (C[ii]) { - AAj = 0; - for (jj=0; jj<n; jj++) if (C[jj]) { - AA[AAi*nskip+AAj] = AROW(ii)[jj]; - AAj++; - } - bb[AAi] = AROW(i)[ii]; - AAi++; - } - if (AAi==0) return; - - dFactorLDLT (AA,dd,AAi,nskip); - dSolveLDLT (AA,dd,bb,AAi,nskip); - - AAi=0; - if (dir > 0) { - for (ii=0; ii<n; ii++) if (C[ii]) a[ii] = -bb[AAi++]; - } - else { - for (ii=0; ii<n; ii++) if (C[ii]) a[ii] = bb[AAi++]; - } -} - - -void dLCP::unpermute() -{ -} - -#endif // dLCP_SLOW - -//*************************************************************************** -// fast implementation of dLCP. see the above definition of dLCP for -// interface comments. -// -// `p' records the permutation of A,x,b,w,etc. p is initially 1:n and is -// permuted as the other vectors/matrices are permuted. -// -// A,x,b,w,lo,hi,state,findex,p,c are permuted such that sets C,N have -// contiguous indexes. the don't-care indexes follow N. -// -// an L*D*L' factorization is maintained of A(C,C), and whenever indexes are -// added or removed from the set C the factorization is updated. -// thus L*D*L'=A[C,C], i.e. a permuted top left nC*nC submatrix of A. -// the leading dimension of the matrix L is always `nskip'. -// -// at the start there may be other indexes that are unbounded but are not -// included in `nub'. dLCP will permute the matrix so that absolutely all -// unbounded vectors are at the start. thus there may be some initial -// permutation. -// -// the algorithms here assume certain patterns, particularly with respect to -// index transfer. - -#ifdef dLCP_FAST - -struct dLCP { - int n,nskip,nub; - ATYPE A; // A rows - dReal *Adata,*x,*b,*w,*lo,*hi; // permuted LCP problem data - dReal *L,*d; // L*D*L' factorization of set C - dReal *Dell,*ell,*tmp; - int *state,*findex,*p,*C; - int nC,nN; // size of each index set - - dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w, - dReal *_lo, dReal *_hi, dReal *_L, dReal *_d, - dReal *_Dell, dReal *_ell, dReal *_tmp, - int *_state, int *_findex, int *_p, int *_C, dReal **Arows); - int getNub() { return nub; } - void transfer_i_to_C (int i); - void transfer_i_to_N (int i) - { nN++; } // because we can assume C and N span 1:i-1 - void transfer_i_from_N_to_C (int i); - void transfer_i_from_C_to_N (int i); - int numC() { return nC; } - int numN() { return nN; } - int indexC (int i) { return i; } - int indexN (int i) { return i+nC; } - dReal Aii (int i) { return AROW(i)[i]; } - dReal AiC_times_qC (int i, dReal *q) { return dDot (AROW(i),q,nC); } - dReal AiN_times_qN (int i, dReal *q) { return dDot (AROW(i)+nC,q+nC,nN); } - void pN_equals_ANC_times_qC (dReal *p, dReal *q); - void pN_plusequals_ANi (dReal *p, int i, int sign=1); - void pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q) - { for (int i=0; i<nC; i++) p[i] += s*q[i]; } - void pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q) - { for (int i=0; i<nN; i++) p[i+nC] += s*q[i+nC]; } - void solve1 (dReal *a, int i, int dir=1, int only_transfer=0); - void unpermute(); -}; - - -dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w, - dReal *_lo, dReal *_hi, dReal *_L, dReal *_d, - dReal *_Dell, dReal *_ell, dReal *_tmp, - int *_state, int *_findex, int *_p, int *_C, dReal **Arows) -{ - n = _n; - nub = _nub; - Adata = _Adata; - A = 0; - x = _x; - b = _b; - w = _w; - lo = _lo; - hi = _hi; - L = _L; - d = _d; - Dell = _Dell; - ell = _ell; - tmp = _tmp; - state = _state; - findex = _findex; - p = _p; - C = _C; - nskip = dPAD(n); - dSetZero (x,n); - - int k; - -# ifdef ROWPTRS - // make matrix row pointers - A = Arows; - for (k=0; k<n; k++) A[k] = Adata + k*nskip; -# else - A = Adata; -# endif - - nC = 0; - nN = 0; - for (k=0; k<n; k++) p[k]=k; // initially unpermuted - - /* - // for testing, we can do some random swaps in the area i > nub - if (nub < n) { - for (k=0; k<100; k++) { - int i1,i2; - do { - i1 = dRandInt(n-nub)+nub; - i2 = dRandInt(n-nub)+nub; - } - while (i1 > i2); - //printf ("--> %d %d\n",i1,i2); - swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i1,i2,nskip,0); - } - } - */ - - // permute the problem so that *all* the unbounded variables are at the - // start, i.e. look for unbounded variables not included in `nub'. we can - // potentially push up `nub' this way and get a bigger initial factorization. - // note that when we swap rows/cols here we must not just swap row pointers, - // as the initial factorization relies on the data being all in one chunk. - for (k=nub; k<n; k++) { - if (lo[k]==-dInfinity && hi[k]==dInfinity) { - swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nub,k,nskip,0); - nub++; - } - } - - // if there are unbounded variables at the start, factorize A up to that - // point and solve for x. this puts all indexes 0..nub-1 into C. - if (nub > 0) { - for (k=0; k<nub; k++) memcpy (L+k*nskip,AROW(k),(k+1)*sizeof(dReal)); - dFactorLDLT (L,d,nub,nskip); - memcpy (x,b,nub*sizeof(dReal)); - dSolveLDLT (L,d,x,nub,nskip); - dSetZero (w,nub); - for (k=0; k<nub; k++) C[k] = k; - nC = nub; - } - - // permute the indexes > nub such that all findex variables are at the end - if (findex) { - int num_at_end = 0; - for (k=n-1; k >= nub; k--) { - if (findex[k] >= 0) { - swapProblem (A,x,b,w,lo,hi,p,state,findex,n,k,n-1-num_at_end,nskip,1); - num_at_end++; - } - } - } - - // print info about indexes - /* - for (k=0; k<n; k++) { - if (k<nub) printf ("C"); - else if (lo[k]==-dInfinity && hi[k]==dInfinity) printf ("c"); - else printf ("."); - } - printf ("\n"); - */ -} - - -void dLCP::transfer_i_to_C (int i) -{ - int j; - if (nC > 0) { - // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C)) - for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j]; - d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC)); - } - else { - d[0] = dRecip (AROW(i)[i]); - } - swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1); - C[nC] = nC; - nC++; - -# ifdef DEBUG_LCP - checkFactorization (A,L,d,nC,C,nskip); - if (i < (n-1)) checkPermutations (i+1,n,nC,nN,p,C); -# endif -} - - -void dLCP::transfer_i_from_N_to_C (int i) -{ - int j; - if (nC > 0) { - dReal *aptr = AROW(i); -# ifdef NUB_OPTIMIZATIONS - // if nub>0, initial part of aptr unpermuted - for (j=0; j<nub; j++) Dell[j] = aptr[j]; - for (j=nub; j<nC; j++) Dell[j] = aptr[C[j]]; -# else - for (j=0; j<nC; j++) Dell[j] = aptr[C[j]]; -# endif - dSolveL1 (L,Dell,nC,nskip); - for (j=0; j<nC; j++) ell[j] = Dell[j] * d[j]; - for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j]; - d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC)); - } - else { - d[0] = dRecip (AROW(i)[i]); - } - swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1); - C[nC] = nC; - nN--; - nC++; - - // @@@ TO DO LATER - // if we just finish here then we'll go back and re-solve for - // delta_x. but actually we can be more efficient and incrementally - // update delta_x here. but if we do this, we wont have ell and Dell - // to use in updating the factorization later. - -# ifdef DEBUG_LCP - checkFactorization (A,L,d,nC,C,nskip); -# endif -} - - -void dLCP::transfer_i_from_C_to_N (int i) -{ - // remove a row/column from the factorization, and adjust the - // indexes (black magic!) - int j,k; - for (j=0; j<nC; j++) if (C[j]==i) { - dLDLTRemove (A,C,L,d,n,nC,j,nskip); - for (k=0; k<nC; k++) if (C[k]==nC-1) { - C[k] = C[j]; - if (j < (nC-1)) memmove (C+j,C+j+1,(nC-j-1)*sizeof(int)); - break; - } - dIASSERT (k < nC); - break; - } - dIASSERT (j < nC); - swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i,nC-1,nskip,1); - nC--; - nN++; - -# ifdef DEBUG_LCP - checkFactorization (A,L,d,nC,C,nskip); -# endif -} - - -void dLCP::pN_equals_ANC_times_qC (dReal *p, dReal *q) -{ - // we could try to make this matrix-vector multiplication faster using - // outer product matrix tricks, e.g. with the dMultidotX() functions. - // but i tried it and it actually made things slower on random 100x100 - // problems because of the overhead involved. so we'll stick with the - // simple method for now. - for (int i=0; i<nN; i++) p[i+nC] = dDot (AROW(i+nC),q,nC); -} - - -void dLCP::pN_plusequals_ANi (dReal *p, int i, int sign) -{ - dReal *aptr = AROW(i)+nC; - if (sign > 0) { - for (int i=0; i<nN; i++) p[i+nC] += aptr[i]; - } - else { - for (int i=0; i<nN; i++) p[i+nC] -= aptr[i]; - } -} - - -void dLCP::solve1 (dReal *a, int i, int dir, int only_transfer) -{ - // the `Dell' and `ell' that are computed here are saved. if index i is - // later added to the factorization then they can be reused. - // - // @@@ question: do we need to solve for entire delta_x??? yes, but - // only if an x goes below 0 during the step. - - int j; - if (nC > 0) { - dReal *aptr = AROW(i); -# ifdef NUB_OPTIMIZATIONS - // if nub>0, initial part of aptr[] is guaranteed unpermuted - for (j=0; j<nub; j++) Dell[j] = aptr[j]; - for (j=nub; j<nC; j++) Dell[j] = aptr[C[j]]; -# else - for (j=0; j<nC; j++) Dell[j] = aptr[C[j]]; -# endif - dSolveL1 (L,Dell,nC,nskip); - for (j=0; j<nC; j++) ell[j] = Dell[j] * d[j]; - - if (!only_transfer) { - for (j=0; j<nC; j++) tmp[j] = ell[j]; - dSolveL1T (L,tmp,nC,nskip); - if (dir > 0) { - for (j=0; j<nC; j++) a[C[j]] = -tmp[j]; - } - else { - for (j=0; j<nC; j++) a[C[j]] = tmp[j]; - } - } - } -} - - -void dLCP::unpermute() -{ - // now we have to un-permute x and w - int j; - dReal *tmp = (dReal*) ALLOCA (n*sizeof(dReal)); - memcpy (tmp,x,n*sizeof(dReal)); - for (j=0; j<n; j++) x[p[j]] = tmp[j]; - memcpy (tmp,w,n*sizeof(dReal)); - for (j=0; j<n; j++) w[p[j]] = tmp[j]; -} - -#endif // dLCP_FAST - -//*************************************************************************** -// an unoptimized Dantzig LCP driver routine for the basic LCP problem. -// must have lo=0, hi=dInfinity, and nub=0. - -void dSolveLCPBasic (int n, dReal *A, dReal *x, dReal *b, - dReal *w, int nub, dReal *lo, dReal *hi) -{ - dAASSERT (n>0 && A && x && b && w && nub == 0); - - int i,k; - int nskip = dPAD(n); - dReal *L = (dReal*) ALLOCA (n*nskip*sizeof(dReal)); - dReal *d = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *delta_x = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *delta_w = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *Dell = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *ell = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *tmp = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal **Arows = (dReal**) ALLOCA (n*sizeof(dReal*)); - int *p = (int*) ALLOCA (n*sizeof(int)); - int *C = (int*) ALLOCA (n*sizeof(int)); - int *dummy = (int*) ALLOCA (n*sizeof(int)); - - dLCP lcp (n,0,A,x,b,w,tmp,tmp,L,d,Dell,ell,tmp,dummy,dummy,p,C,Arows); - nub = lcp.getNub(); - - for (i=0; i<n; i++) { - w[i] = lcp.AiC_times_qC (i,x) - b[i]; - if (w[i] >= 0) { - lcp.transfer_i_to_N (i); - } - else { - for (;;) { - // compute: delta_x(C) = -A(C,C)\A(C,i) - dSetZero (delta_x,n); - lcp.solve1 (delta_x,i); - delta_x[i] = 1; - - // compute: delta_w = A*delta_x - dSetZero (delta_w,n); - lcp.pN_equals_ANC_times_qC (delta_w,delta_x); - lcp.pN_plusequals_ANi (delta_w,i); - delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i); - - // find index to switch - int si = i; // si = switch index - int si_in_N = 0; // set to 1 if si in N - dReal s = -w[i]/delta_w[i]; - - if (s <= 0) { - dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s); - if (i < (n-1)) { - dSetZero (x+i,n-i); - dSetZero (w+i,n-i); - } - goto done; - } - - for (k=0; k < lcp.numN(); k++) { - if (delta_w[lcp.indexN(k)] < 0) { - dReal s2 = -w[lcp.indexN(k)] / delta_w[lcp.indexN(k)]; - if (s2 < s) { - s = s2; - si = lcp.indexN(k); - si_in_N = 1; - } - } - } - for (k=0; k < lcp.numC(); k++) { - if (delta_x[lcp.indexC(k)] < 0) { - dReal s2 = -x[lcp.indexC(k)] / delta_x[lcp.indexC(k)]; - if (s2 < s) { - s = s2; - si = lcp.indexC(k); - si_in_N = 0; - } - } - } - - // apply x = x + s * delta_x - lcp.pC_plusequals_s_times_qC (x,s,delta_x); - x[i] += s; - lcp.pN_plusequals_s_times_qN (w,s,delta_w); - w[i] += s * delta_w[i]; - - // switch indexes between sets if necessary - if (si==i) { - w[i] = 0; - lcp.transfer_i_to_C (i); - break; - } - if (si_in_N) { - w[si] = 0; - lcp.transfer_i_from_N_to_C (si); - } - else { - x[si] = 0; - lcp.transfer_i_from_C_to_N (si); - } - } - } - } - - done: - lcp.unpermute(); -} - -//*************************************************************************** -// an optimized Dantzig LCP driver routine for the lo-hi LCP problem. - -void dSolveLCP (int n, dReal *A, dReal *x, dReal *b, - dReal *w, int nub, dReal *lo, dReal *hi, int *findex) -{ - dAASSERT (n>0 && A && x && b && w && lo && hi && nub >= 0 && nub <= n); - int i,k,hit_first_friction_index = 0; - int nskip = dPAD(n); - - // if all the variables are unbounded then we can just factor, solve, - // and return - if (nub >= n) { - dFactorLDLT (A,w,n,nskip); // use w for d - dSolveLDLT (A,w,b,n,nskip); - memcpy (x,b,n*sizeof(dReal)); - dSetZero (w,n); - return; - } - -# ifndef dNODEBUG - // check restrictions on lo and hi - for (k=0; k<n; k++) dIASSERT (lo[k] <= 0 && hi[k] >= 0); -# endif - - dReal *L = (dReal*) ALLOCA (n*nskip*sizeof(dReal)); - dReal *d = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *delta_x = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *delta_w = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *Dell = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *ell = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal **Arows = (dReal**) ALLOCA (n*sizeof(dReal*)); - int *p = (int*) ALLOCA (n*sizeof(int)); - int *C = (int*) ALLOCA (n*sizeof(int)); - int dir; - dReal dirf; - - // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i) - int *state = (int*) ALLOCA (n*sizeof(int)); - - // create LCP object. note that tmp is set to delta_w to save space, this - // optimization relies on knowledge of how tmp is used, so be careful! - dLCP lcp (n,nub,A,x,b,w,lo,hi,L,d,Dell,ell,delta_w,state,findex,p,C,Arows); - nub = lcp.getNub(); - - // loop over all indexes nub..n-1. for index i, if x(i),w(i) satisfy the - // LCP conditions then i is added to the appropriate index set. otherwise - // x(i),w(i) is driven either +ve or -ve to force it to the valid region. - // as we drive x(i), x(C) is also adjusted to keep w(C) at zero. - // while driving x(i) we maintain the LCP conditions on the other variables - // 0..i-1. we do this by watching out for other x(i),w(i) values going - // outside the valid region, and then switching them between index sets - // when that happens. - - for (i=nub; i<n; i++) { - // the index i is the driving index and indexes i+1..n-1 are "dont care", - // i.e. when we make changes to the system those x's will be zero and we - // don't care what happens to those w's. in other words, we only consider - // an (i+1)*(i+1) sub-problem of A*x=b+w. - - // if we've hit the first friction index, we have to compute the lo and - // hi values based on the values of x already computed. we have been - // permuting the indexes, so the values stored in the findex vector are - // no longer valid. thus we have to temporarily unpermute the x vector. - if (hit_first_friction_index == 0 && findex && findex[i] >= 0) { - // un-permute x into delta_w, which is not being used at the moment - for (k=0; k<n; k++) delta_w[p[k]] = x[k]; - // set lo and hi values - for (k=i; k<n; k++) { - hi[k] = dFabs (hi[k] * delta_w[findex[k]]); - lo[k] = -hi[k]; - } - hit_first_friction_index = 1; - } - - // thus far we have not even been computing the w values for indexes - // greater than i, so compute w[i] now. - w[i] = lcp.AiC_times_qC (i,x) + lcp.AiN_times_qN (i,x) - b[i]; - - // if lo=hi=0 (which can happen for tangential friction when normals are - // 0) then the index will be assigned to set N with some state. however, - // set C's line has zero size, so the index will always remain in set N. - // with the "normal" switching logic, if w changed sign then the index - // would have to switch to set C and then back to set N with an inverted - // state. this is pointless, and also computationally expensive. to - // prevent this from happening, we use the rule that indexes with lo=hi=0 - // will never be checked for set changes. this means that the state for - // these indexes may be incorrect, but that doesn't matter. - - // see if x(i),w(i) is in a valid region - if (lo[i]==0 && w[i] >= 0) { - lcp.transfer_i_to_N (i); - state[i] = 0; - } - else if (hi[i]==0 && w[i] <= 0) { - lcp.transfer_i_to_N (i); - state[i] = 1; - } - else if (w[i]==0) { - // this is a degenerate case. by the time we get to this test we know - // that lo != 0, which means that lo < 0 as lo is not allowed to be +ve, - // and similarly that hi > 0. this means that the line segment - // corresponding to set C is at least finite in extent, and we are on it. - // NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C() - lcp.solve1 (delta_x,i,0,1); - lcp.transfer_i_to_C (i); - } - else { - // we must push x(i) and w(i) - for (;;) { - // find direction to push on x(i) - if (w[i] <= 0) { - dir = 1; - dirf = REAL(1.0); - } - else { - dir = -1; - dirf = REAL(-1.0); - } - - // compute: delta_x(C) = -dir*A(C,C)\A(C,i) - lcp.solve1 (delta_x,i,dir); - // note that delta_x[i] = dirf, but we wont bother to set it - - // compute: delta_w = A*delta_x ... note we only care about - // delta_w(N) and delta_w(i), the rest is ignored - lcp.pN_equals_ANC_times_qC (delta_w,delta_x); - lcp.pN_plusequals_ANi (delta_w,i,dir); - delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i)*dirf; - - // find largest step we can take (size=s), either to drive x(i),w(i) - // to the valid LCP region or to drive an already-valid variable - // outside the valid region. - - int cmd = 1; // index switching command - int si = 0; // si = index to switch if cmd>3 - dReal s = -w[i]/delta_w[i]; - if (dir > 0) { - if (hi[i] < dInfinity) { - dReal s2 = (hi[i]-x[i])/dirf; // step to x(i)=hi(i) - if (s2 < s) { - s = s2; - cmd = 3; - } - } - } - else { - if (lo[i] > -dInfinity) { - dReal s2 = (lo[i]-x[i])/dirf; // step to x(i)=lo(i) - if (s2 < s) { - s = s2; - cmd = 2; - } - } - } - - for (k=0; k < lcp.numN(); k++) { - if ((state[lcp.indexN(k)]==0 && delta_w[lcp.indexN(k)] < 0) || - (state[lcp.indexN(k)]!=0 && delta_w[lcp.indexN(k)] > 0)) { - // don't bother checking if lo=hi=0 - if (lo[lcp.indexN(k)] == 0 && hi[lcp.indexN(k)] == 0) continue; - dReal s2 = -w[lcp.indexN(k)] / delta_w[lcp.indexN(k)]; - if (s2 < s) { - s = s2; - cmd = 4; - si = lcp.indexN(k); - } - } - } - - for (k=nub; k < lcp.numC(); k++) { - if (delta_x[lcp.indexC(k)] < 0 && lo[lcp.indexC(k)] > -dInfinity) { - dReal s2 = (lo[lcp.indexC(k)]-x[lcp.indexC(k)]) / - delta_x[lcp.indexC(k)]; - if (s2 < s) { - s = s2; - cmd = 5; - si = lcp.indexC(k); - } - } - if (delta_x[lcp.indexC(k)] > 0 && hi[lcp.indexC(k)] < dInfinity) { - dReal s2 = (hi[lcp.indexC(k)]-x[lcp.indexC(k)]) / - delta_x[lcp.indexC(k)]; - if (s2 < s) { - s = s2; - cmd = 6; - si = lcp.indexC(k); - } - } - } - - //static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C", - // "C->NL","C->NH"}; - //printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i); - - // if s <= 0 then we've got a problem. if we just keep going then - // we're going to get stuck in an infinite loop. instead, just cross - // our fingers and exit with the current solution. - if (s <= 0) { - dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s); - if (i < (n-1)) { - dSetZero (x+i,n-i); - dSetZero (w+i,n-i); - } - goto done; - } - - // apply x = x + s * delta_x - lcp.pC_plusequals_s_times_qC (x,s,delta_x); - x[i] += s * dirf; - - // apply w = w + s * delta_w - lcp.pN_plusequals_s_times_qN (w,s,delta_w); - w[i] += s * delta_w[i]; - - // switch indexes between sets if necessary - switch (cmd) { - case 1: // done - w[i] = 0; - lcp.transfer_i_to_C (i); - break; - case 2: // done - x[i] = lo[i]; - state[i] = 0; - lcp.transfer_i_to_N (i); - break; - case 3: // done - x[i] = hi[i]; - state[i] = 1; - lcp.transfer_i_to_N (i); - break; - case 4: // keep going - w[si] = 0; - lcp.transfer_i_from_N_to_C (si); - break; - case 5: // keep going - x[si] = lo[si]; - state[si] = 0; - lcp.transfer_i_from_C_to_N (si); - break; - case 6: // keep going - x[si] = hi[si]; - state[si] = 1; - lcp.transfer_i_from_C_to_N (si); - break; - } - - if (cmd <= 3) break; - } - } - } - - done: - lcp.unpermute(); -} - -//*************************************************************************** -// accuracy and timing test - -extern "C" void dTestSolveLCP() -{ - int n = 100; - int i,nskip = dPAD(n); - const dReal tol = REAL(1e-9); - printf ("dTestSolveLCP()\n"); - - dReal *A = (dReal*) ALLOCA (n*nskip*sizeof(dReal)); - dReal *x = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *b = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *w = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *lo = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *hi = (dReal*) ALLOCA (n*sizeof(dReal)); - - dReal *A2 = (dReal*) ALLOCA (n*nskip*sizeof(dReal)); - dReal *b2 = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *lo2 = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *hi2 = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *tmp1 = (dReal*) ALLOCA (n*sizeof(dReal)); - dReal *tmp2 = (dReal*) ALLOCA (n*sizeof(dReal)); - - double total_time = 0; - for (int count=0; count < 1000; count++) { - - // form (A,b) = a random positive definite LCP problem - dMakeRandomMatrix (A2,n,n,1.0); - dMultiply2 (A,A2,A2,n,n,n); - dMakeRandomMatrix (x,n,1,1.0); - dMultiply0 (b,A,x,n,n,1); - for (i=0; i<n; i++) b[i] += (dRandReal()*REAL(0.2))-REAL(0.1); - - // choose `nub' in the range 0..n-1 - int nub = 50; //dRandInt (n); - - // make limits - for (i=0; i<nub; i++) lo[i] = -dInfinity; - for (i=0; i<nub; i++) hi[i] = dInfinity; - //for (i=nub; i<n; i++) lo[i] = 0; - //for (i=nub; i<n; i++) hi[i] = dInfinity; - //for (i=nub; i<n; i++) lo[i] = -dInfinity; - //for (i=nub; i<n; i++) hi[i] = 0; - for (i=nub; i<n; i++) lo[i] = -(dRandReal()*REAL(1.0))-REAL(0.01); - for (i=nub; i<n; i++) hi[i] = (dRandReal()*REAL(1.0))+REAL(0.01); - - // set a few limits to lo=hi=0 - /* - for (i=0; i<10; i++) { - int j = dRandInt (n-nub) + nub; - lo[j] = 0; - hi[j] = 0; - } - */ - - // solve the LCP. we must make copy of A,b,lo,hi (A2,b2,lo2,hi2) for - // SolveLCP() to permute. also, we'll clear the upper triangle of A2 to - // ensure that it doesn't get referenced (if it does, the answer will be - // wrong). - - memcpy (A2,A,n*nskip*sizeof(dReal)); - dClearUpperTriangle (A2,n); - memcpy (b2,b,n*sizeof(dReal)); - memcpy (lo2,lo,n*sizeof(dReal)); - memcpy (hi2,hi,n*sizeof(dReal)); - dSetZero (x,n); - dSetZero (w,n); - - dStopwatch sw; - dStopwatchReset (&sw); - dStopwatchStart (&sw); - - dSolveLCP (n,A2,x,b2,w,nub,lo2,hi2,0); - - dStopwatchStop (&sw); - double time = dStopwatchTime(&sw); - total_time += time; - double average = total_time / double(count+1) * 1000.0; - - // check the solution - - dMultiply0 (tmp1,A,x,n,n,1); - for (i=0; i<n; i++) tmp2[i] = b[i] + w[i]; - dReal diff = dMaxDifference (tmp1,tmp2,n,1); - // printf ("\tA*x = b+w, maximum difference = %.6e - %s (1)\n",diff, - // diff > tol ? "FAILED" : "passed"); - if (diff > tol) dDebug (0,"A*x = b+w, maximum difference = %.6e",diff); - int n1=0,n2=0,n3=0; - for (i=0; i<n; i++) { - if (x[i]==lo[i] && w[i] >= 0) { - n1++; // ok - } - else if (x[i]==hi[i] && w[i] <= 0) { - n2++; // ok - } - else if (x[i] >= lo[i] && x[i] <= hi[i] && w[i] == 0) { - n3++; // ok - } - else { - dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i, - x[i],w[i],lo[i],hi[i]); - } - } - - // pacifier - printf ("passed: NL=%3d NH=%3d C=%3d ",n1,n2,n3); - printf ("time=%10.3f ms avg=%10.4f\n",time * 1000.0,average); - } -} diff --git a/extern/ode/dist/ode/src/lcp.h b/extern/ode/dist/ode/src/lcp.h deleted file mode 100644 index 86cbd6319eb..00000000000 --- a/extern/ode/dist/ode/src/lcp.h +++ /dev/null @@ -1,59 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -/* - -given (A,b,lo,hi), solve the LCP problem: A*x = b+w, where each x(i),w(i) -satisfies one of - (1) x = lo, w >= 0 - (2) x = hi, w <= 0 - (3) lo < x < hi, w = 0 -A is a matrix of dimension n*n, everything else is a vector of size n*1. -lo and hi can be +/- dInfinity as needed. the first `nub' variables are -unbounded, i.e. hi and lo are assumed to be +/- dInfinity. - -we restrict lo(i) <= 0 and hi(i) >= 0. - -the original data (A,b) may be modified by this function. - -if the `findex' (friction index) parameter is nonzero, it points to an array -of index values. in this case constraints that have findex[i] >= 0 are -special. all non-special constraints are solved for, then the lo and hi values -for the special constraints are set: - hi[i] = abs( hi[i] * x[findex[i]] ) - lo[i] = -hi[i] -and the solution continues. this mechanism allows a friction approximation -to be implemented. - -*/ - - -#ifndef _ODE_LCP_H_ -#define _ODE_LCP_H_ - - -void dSolveLCP (int n, dReal *A, dReal *x, dReal *b, dReal *w, - int nub, dReal *lo, dReal *hi, int *findex); - - -#endif - diff --git a/extern/ode/dist/ode/src/mass.cpp b/extern/ode/dist/ode/src/mass.cpp deleted file mode 100644 index 9c1aae2033f..00000000000 --- a/extern/ode/dist/ode/src/mass.cpp +++ /dev/null @@ -1,261 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#include <ode/config.h> -#include <ode/mass.h> -#include <ode/odemath.h> -#include <ode/matrix.h> - - -#define _I(i,j) I[(i)*4+(j)] - - -// return 1 if ok, 0 if bad - -static int checkMass (dMass *m) -{ - if (m->mass <= 0) { - dDEBUGMSG ("mass must be > 0"); - return 0; - } - if (!dIsPositiveDefinite (m->I,3)) { - dDEBUGMSG ("inertia must be positive definite"); - return 0; - } - - // verify that the center of mass position is consistent with the mass - // and inertia matrix. this is done by checking that the inertia around - // the center of mass is also positive definite. from the comment in - // dMassTranslate(), if the body is translated so that its center of mass - // is at the point of reference, then the new inertia is: - // I + mass*crossmat(c)^2 - // note that requiring this to be positive definite is exactly equivalent - // to requiring that the spatial inertia matrix - // [ mass*eye(3,3) M*crossmat(c)^T ] - // [ M*crossmat(c) I ] - // is positive definite, given that I is PD and mass>0. see the theorem - // about partitioned PD matrices for proof. - - dMatrix3 I2,chat; - dSetZero (chat,12); - dCROSSMAT (chat,m->c,4,+,-); - dMULTIPLY0_333 (I2,chat,chat); - for (int i=0; i<12; i++) I2[i] = m->I[i] + m->mass*I2[i]; - if (!dIsPositiveDefinite (I2,3)) { - dDEBUGMSG ("center of mass inconsistent with mass parameters"); - return 0; - } - return 1; -} - - -void dMassSetZero (dMass *m) -{ - dAASSERT (m); - m->mass = REAL(0.0); - dSetZero (m->c,sizeof(m->c) / sizeof(dReal)); - dSetZero (m->I,sizeof(m->I) / sizeof(dReal)); -} - - -void dMassSetParameters (dMass *m, dReal themass, - dReal cgx, dReal cgy, dReal cgz, - dReal I11, dReal I22, dReal I33, - dReal I12, dReal I13, dReal I23) -{ - dAASSERT (m); - dMassSetZero (m); - m->mass = themass; - m->c[0] = cgx; - m->c[1] = cgy; - m->c[2] = cgz; - m->_I(0,0) = I11; - m->_I(1,1) = I22; - m->_I(2,2) = I33; - m->_I(0,1) = I12; - m->_I(0,2) = I13; - m->_I(1,2) = I23; - m->_I(1,0) = I12; - m->_I(2,0) = I13; - m->_I(2,1) = I23; - checkMass (m); -} - - -void dMassSetSphere (dMass *m, dReal density, dReal radius) -{ - dAASSERT (m); - dMassSetZero (m); - m->mass = (REAL(4.0)/REAL(3.0)) * M_PI * radius*radius*radius * density; - dReal II = REAL(0.4) * m->mass * radius*radius; - m->_I(0,0) = II; - m->_I(1,1) = II; - m->_I(2,2) = II; - -# ifndef dNODEBUG - checkMass (m); -# endif -} - - -void dMassSetCappedCylinder (dMass *m, dReal density, int direction, - dReal a, dReal b) -{ - dReal M1,M2,Ia,Ib; - dAASSERT (m); - dUASSERT (direction >= 1 && direction <= 3,"bad direction number"); - dMassSetZero (m); - M1 = M_PI*a*a*b*density; // cylinder mass - M2 = (REAL(4.0)/REAL(3.0))*M_PI*a*a*a*density; // total cap mass - m->mass = M1+M2; - Ia = M1*(REAL(0.25)*a*a + (REAL(1.0)/REAL(12.0))*b*b) + - M2*(REAL(0.4)*a*a + REAL(0.5)*b*b); - Ib = (M1*REAL(0.5) + M2*REAL(0.4))*a*a; - m->_I(0,0) = Ia; - m->_I(1,1) = Ia; - m->_I(2,2) = Ia; - m->_I(direction-1,direction-1) = Ib; - -# ifndef dNODEBUG - checkMass (m); -# endif -} - - -void dMassSetBox (dMass *m, dReal density, - dReal lx, dReal ly, dReal lz) -{ - dAASSERT (m); - dMassSetZero (m); - dReal M = lx*ly*lz*density; - m->mass = M; - m->_I(0,0) = M/REAL(12.0) * (ly*ly + lz*lz); - m->_I(1,1) = M/REAL(12.0) * (lx*lx + lz*lz); - m->_I(2,2) = M/REAL(12.0) * (lx*lx + ly*ly); - -# ifndef dNODEBUG - checkMass (m); -# endif -} - - -void dMassAdjust (dMass *m, dReal newmass) -{ - dAASSERT (m); - dReal scale = newmass / m->mass; - m->mass = newmass; - for (int i=0; i<3; i++) for (int j=0; j<3; j++) m->_I(i,j) *= scale; - -# ifndef dNODEBUG - checkMass (m); -# endif -} - - -void dMassTranslate (dMass *m, dReal x, dReal y, dReal z) -{ - // if the body is translated by `a' relative to its point of reference, - // the new inertia about the point of reference is: - // - // I + mass*(crossmat(c)^2 - crossmat(c+a)^2) - // - // where c is the existing center of mass and I is the old inertia. - - int i,j; - dMatrix3 ahat,chat,t1,t2; - dReal a[3]; - - dAASSERT (m); - - // adjust inertia matrix - dSetZero (chat,12); - dCROSSMAT (chat,m->c,4,+,-); - a[0] = x + m->c[0]; - a[1] = y + m->c[1]; - a[2] = z + m->c[2]; - dSetZero (ahat,12); - dCROSSMAT (ahat,a,4,+,-); - dMULTIPLY0_333 (t1,ahat,ahat); - dMULTIPLY0_333 (t2,chat,chat); - for (i=0; i<3; i++) for (j=0; j<3; j++) - m->_I(i,j) += m->mass * (t2[i*4+j]-t1[i*4+j]); - - // ensure perfect symmetry - m->_I(1,0) = m->_I(0,1); - m->_I(2,0) = m->_I(0,2); - m->_I(2,1) = m->_I(1,2); - - // adjust center of mass - m->c[0] += x; - m->c[1] += y; - m->c[2] += z; - -# ifndef dNODEBUG - checkMass (m); -# endif -} - - -void dMassRotate (dMass *m, const dMatrix3 R) -{ - // if the body is rotated by `R' relative to its point of reference, - // the new inertia about the point of reference is: - // - // R * I * R' - // - // where I is the old inertia. - - dMatrix3 t1; - dReal t2[3]; - - dAASSERT (m); - - // rotate inertia matrix - dMULTIPLY2_333 (t1,m->I,R); - dMULTIPLY0_333 (m->I,R,t1); - - // ensure perfect symmetry - m->_I(1,0) = m->_I(0,1); - m->_I(2,0) = m->_I(0,2); - m->_I(2,1) = m->_I(1,2); - - // rotate center of mass - dMULTIPLY0_331 (t2,R,m->c); - m->c[0] = t2[0]; - m->c[1] = t2[1]; - m->c[2] = t2[2]; - -# ifndef dNODEBUG - checkMass (m); -# endif -} - - -void dMassAdd (dMass *a, const dMass *b) -{ - int i; - dAASSERT (a && b); - dReal denom = dRecip (a->mass + b->mass); - for (i=0; i<3; i++) a->c[i] = (a->c[i]*a->mass + b->c[i]*b->mass)*denom; - a->mass += b->mass; - for (i=0; i<12; i++) a->I[i] += b->I[i]; -} diff --git a/extern/ode/dist/ode/src/mat.cpp b/extern/ode/dist/ode/src/mat.cpp deleted file mode 100644 index 6e635dcc994..00000000000 --- a/extern/ode/dist/ode/src/mat.cpp +++ /dev/null @@ -1,230 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#include <ode/config.h> -#include <ode/misc.h> -#include <ode/matrix.h> -#include <ode/error.h> -#include <ode/memory.h> -#include "mat.h" - - -dMatrix::dMatrix() -{ - n = 0; - m = 0; - data = 0; -} - - -dMatrix::dMatrix (int rows, int cols) -{ - if (rows < 1 || cols < 1) dDebug (0,"bad matrix size"); - n = rows; - m = cols; - data = (dReal*) dAlloc (n*m*sizeof(dReal)); - dSetZero (data,n*m); -} - - -dMatrix::dMatrix (const dMatrix &a) -{ - n = a.n; - m = a.m; - data = (dReal*) dAlloc (n*m*sizeof(dReal)); - memcpy (data,a.data,n*m*sizeof(dReal)); -} - - -dMatrix::dMatrix (int rows, int cols, - dReal *_data, int rowskip, int colskip) -{ - if (rows < 1 || cols < 1) dDebug (0,"bad matrix size"); - n = rows; - m = cols; - data = (dReal*) dAlloc (n*m*sizeof(dReal)); - for (int i=0; i<n; i++) { - for (int j=0; j<m; j++) data[i*m+j] = _data[i*rowskip + j*colskip]; - } -} - - -dMatrix::~dMatrix() -{ - if (data) dFree (data,n*m*sizeof(dReal)); -} - - -dReal & dMatrix::operator () (int i, int j) -{ - if (i < 0 || i >= n || j < 0 || j >= m) dDebug (0,"bad matrix (i,j)"); - return data [i*m+j]; -} - - -void dMatrix::operator= (const dMatrix &a) -{ - if (data) dFree (data,n*m*sizeof(dReal)); - n = a.n; - m = a.m; - if (n > 0 && m > 0) { - data = (dReal*) dAlloc (n*m*sizeof(dReal)); - memcpy (data,a.data,n*m*sizeof(dReal)); - } - else data = 0; -} - - -void dMatrix::operator= (dReal a) -{ - for (int i=0; i<n*m; i++) data[i] = a; -} - - -dMatrix dMatrix::transpose() -{ - dMatrix r (m,n); - for (int i=0; i<n; i++) { - for (int j=0; j<m; j++) r.data[j*n+i] = data[i*m+j]; - } - return r; -} - - -dMatrix dMatrix::select (int np, int *p, int nq, int *q) -{ - if (np < 1 || nq < 1) dDebug (0,"Matrix select, bad index array sizes"); - dMatrix r (np,nq); - for (int i=0; i<np; i++) { - for (int j=0; j<nq; j++) { - if (p[i] < 0 || p[i] >= n || q[i] < 0 || q[i] >= m) - dDebug (0,"Matrix select, bad index arrays"); - r.data[i*nq+j] = data[p[i]*m+q[j]]; - } - } - return r; -} - - -dMatrix dMatrix::operator + (const dMatrix &a) -{ - if (n != a.n || m != a.m) dDebug (0,"matrix +, mismatched sizes"); - dMatrix r (n,m); - for (int i=0; i<n*m; i++) r.data[i] = data[i] + a.data[i]; - return r; -} - - -dMatrix dMatrix::operator - (const dMatrix &a) -{ - if (n != a.n || m != a.m) dDebug (0,"matrix -, mismatched sizes"); - dMatrix r (n,m); - for (int i=0; i<n*m; i++) r.data[i] = data[i] - a.data[i]; - return r; -} - - -dMatrix dMatrix::operator - () -{ - dMatrix r (n,m); - for (int i=0; i<n*m; i++) r.data[i] = -data[i]; - return r; -} - - -dMatrix dMatrix::operator * (const dMatrix &a) -{ - if (m != a.n) dDebug (0,"matrix *, mismatched sizes"); - dMatrix r (n,a.m); - for (int i=0; i<n; i++) { - for (int j=0; j<a.m; j++) { - dReal sum = 0; - for (int k=0; k<m; k++) sum += data[i*m+k] * a.data[k*a.m+j]; - r.data [i*a.m+j] = sum; - } - } - return r; -} - - -void dMatrix::operator += (const dMatrix &a) -{ - if (n != a.n || m != a.m) dDebug (0,"matrix +=, mismatched sizes"); - for (int i=0; i<n*m; i++) data[i] += a.data[i]; -} - - -void dMatrix::operator -= (const dMatrix &a) -{ - if (n != a.n || m != a.m) dDebug (0,"matrix -=, mismatched sizes"); - for (int i=0; i<n*m; i++) data[i] -= a.data[i]; -} - - -void dMatrix::clearUpperTriangle() -{ - if (n != m) dDebug (0,"clearUpperTriangle() only works on square matrices"); - for (int i=0; i<n; i++) { - for (int j=i+1; j<m; j++) data[i*m+j] = 0; - } -} - - -void dMatrix::clearLowerTriangle() -{ - if (n != m) dDebug (0,"clearLowerTriangle() only works on square matrices"); - for (int i=0; i<n; i++) { - for (int j=0; j<i; j++) data[i*m+j] = 0; - } -} - - -void dMatrix::makeRandom (dReal range) -{ - for (int i=0; i<n; i++) { - for (int j=0; j<m; j++) - data[i*m+j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range; - } -} - - -void dMatrix::print (char *fmt, FILE *f) -{ - for (int i=0; i<n; i++) { - for (int j=0; j<m; j++) fprintf (f,fmt,data[i*m+j]); - fprintf (f,"\n"); - } -} - - -dReal dMatrix::maxDifference (const dMatrix &a) -{ - if (n != a.n || m != a.m) dDebug (0,"maxDifference(), mismatched sizes"); - dReal max = 0; - for (int i=0; i<n; i++) { - for (int j=0; j<m; j++) { - dReal diff = dFabs(data[i*m+j] - a.data[i*m+j]); - if (diff > max) max = diff; - } - } - return max; -} diff --git a/extern/ode/dist/ode/src/mat.h b/extern/ode/dist/ode/src/mat.h deleted file mode 100644 index 26f6bd1093a..00000000000 --- a/extern/ode/dist/ode/src/mat.h +++ /dev/null @@ -1,72 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -// matrix class. this is mostly for convenience in the testing code, it is -// not optimized at all. correctness is much more importance here. - -#ifndef _ODE_MAT_H_ -#define _ODE_MAT_H_ - -#include <ode/common.h> - - -class dMatrix { - int n,m; // matrix dimension, n,m >= 0 - dReal *data; // if nonzero, n*m elements allocated on the heap - -public: - // constructors, destructors - dMatrix(); // make default 0x0 matrix - dMatrix (int rows, int cols); // construct zero matrix of given size - dMatrix (const dMatrix &); // construct copy of given matrix - // create copy of given data - element (i,j) is data[i*rowskip+j*colskip] - dMatrix (int rows, int cols, dReal *_data, int rowskip, int colskip); - ~dMatrix(); // destructor - - // data movement - dReal & operator () (int i, int j); // reference an element - void operator= (const dMatrix &); // matrix = matrix - void operator= (dReal); // matrix = scalar - dMatrix transpose(); // return transposed matrix - // return a permuted submatrix of this matrix, made up of the rows in p - // and the columns in q. p has np elements, q has nq elements. - dMatrix select (int np, int *p, int nq, int *q); - - // operators - dMatrix operator + (const dMatrix &); - dMatrix operator - (const dMatrix &); - dMatrix operator - (); - dMatrix operator * (const dMatrix &); - void operator += (const dMatrix &); - void operator -= (const dMatrix &); - - // utility - void clearUpperTriangle(); - void clearLowerTriangle(); - void makeRandom (dReal range); - void print (char *fmt = "%10.4f ", FILE *f=stdout); - dReal maxDifference (const dMatrix &); -}; - - -#endif - diff --git a/extern/ode/dist/ode/src/matrix.cpp b/extern/ode/dist/ode/src/matrix.cpp deleted file mode 100644 index 16afe915dd6..00000000000 --- a/extern/ode/dist/ode/src/matrix.cpp +++ /dev/null @@ -1,358 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#include <ode/common.h> -#include <ode/matrix.h> - -// misc defines -#define ALLOCA dALLOCA16 - - -void dSetZero (dReal *a, int n) -{ - dAASSERT (a && n >= 0); - while (n > 0) { - *(a++) = 0; - n--; - } -} - - -void dSetValue (dReal *a, int n, dReal value) -{ - dAASSERT (a && n >= 0); - while (n > 0) { - *(a++) = value; - n--; - } -} - - -void dMultiply0 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r) -{ - int i,j,k,qskip,rskip,rpad; - dAASSERT (A && B && C && p>0 && q>0 && r>0); - qskip = dPAD(q); - rskip = dPAD(r); - rpad = rskip - r; - dReal sum; - const dReal *b,*c,*bb; - bb = B; - for (i=p; i; i--) { - for (j=0 ; j<r; j++) { - c = C + j; - b = bb; - sum = 0; - for (k=q; k; k--, c+=rskip) sum += (*(b++))*(*c); - *(A++) = sum; - } - A += rpad; - bb += qskip; - } -} - - -void dMultiply1 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r) -{ - int i,j,k,pskip,rskip; - dReal sum; - dAASSERT (A && B && C && p>0 && q>0 && r>0); - pskip = dPAD(p); - rskip = dPAD(r); - for (i=0; i<p; i++) { - for (j=0; j<r; j++) { - sum = 0; - for (k=0; k<q; k++) sum += B[i+k*pskip] * C[j+k*rskip]; - A[i*rskip+j] = sum; - } - } -} - - -void dMultiply2 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r) -{ - int i,j,k,z,rpad,qskip; - dReal sum; - const dReal *bb,*cc; - dAASSERT (A && B && C && p>0 && q>0 && r>0); - rpad = dPAD(r) - r; - qskip = dPAD(q); - bb = B; - for (i=p; i; i--) { - cc = C; - for (j=r; j; j--) { - z = 0; - sum = 0; - for (k=q; k; k--,z++) sum += bb[z] * cc[z]; - *(A++) = sum; - cc += qskip; - } - A += rpad; - bb += qskip; - } -} - - -int dFactorCholesky (dReal *A, int n) -{ - int i,j,k,nskip; - dReal sum,*a,*b,*aa,*bb,*cc,*recip; - dAASSERT (n > 0 && A); - nskip = dPAD (n); - recip = (dReal*) ALLOCA (n * sizeof(dReal)); - aa = A; - for (i=0; i<n; i++) { - bb = A; - cc = A + i*nskip; - for (j=0; j<i; j++) { - sum = *cc; - a = aa; - b = bb; - for (k=j; k; k--) sum -= (*(a++))*(*(b++)); - *cc = sum * recip[j]; - bb += nskip; - cc++; - } - sum = *cc; - a = aa; - for (k=i; k; k--, a++) sum -= (*a)*(*a); - if (sum <= REAL(0.0)) return 0; - *cc = dSqrt(sum); - recip[i] = dRecip (*cc); - aa += nskip; - } - return 1; -} - - -void dSolveCholesky (const dReal *L, dReal *b, int n) -{ - int i,k,nskip; - dReal sum,*y; - dAASSERT (n > 0 && L && b); - nskip = dPAD (n); - y = (dReal*) ALLOCA (n*sizeof(dReal)); - for (i=0; i<n; i++) { - sum = 0; - for (k=0; k < i; k++) sum += L[i*nskip+k]*y[k]; - y[i] = (b[i]-sum)/L[i*nskip+i]; - } - for (i=n-1; i >= 0; i--) { - sum = 0; - for (k=i+1; k < n; k++) sum += L[k*nskip+i]*b[k]; - b[i] = (y[i]-sum)/L[i*nskip+i]; - } -} - - -int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n) -{ - int i,j,nskip; - dReal *L,*x; - dAASSERT (n > 0 && A && Ainv); - nskip = dPAD (n); - L = (dReal*) ALLOCA (nskip*n*sizeof(dReal)); - memcpy (L,A,nskip*n*sizeof(dReal)); - x = (dReal*) ALLOCA (n*sizeof(dReal)); - if (dFactorCholesky (L,n)==0) return 0; - dSetZero (Ainv,n*nskip); // make sure all padding elements set to 0 - for (i=0; i<n; i++) { - for (j=0; j<n; j++) x[j] = 0; - x[i] = 1; - dSolveCholesky (L,x,n); - for (j=0; j<n; j++) Ainv[j*nskip+i] = x[j]; - } - return 1; -} - - -int dIsPositiveDefinite (const dReal *A, int n) -{ - dReal *Acopy; - dAASSERT (n > 0 && A); - int nskip = dPAD (n); - Acopy = (dReal*) ALLOCA (nskip*n * sizeof(dReal)); - memcpy (Acopy,A,nskip*n * sizeof(dReal)); - return dFactorCholesky (Acopy,n); -} - - -/***** this has been replaced by a faster version -void dSolveL1T (const dReal *L, dReal *b, int n, int nskip) -{ - int i,j; - dAASSERT (L && b && n >= 0 && nskip >= n); - dReal sum; - for (i=n-2; i>=0; i--) { - sum = 0; - for (j=i+1; j<n; j++) sum += L[j*nskip+i]*b[j]; - b[i] -= sum; - } -} -*/ - - -void dVectorScale (dReal *a, const dReal *d, int n) -{ - dAASSERT (a && d && n >= 0); - for (int i=0; i<n; i++) a[i] *= d[i]; -} - - -void dSolveLDLT (const dReal *L, const dReal *d, dReal *b, int n, int nskip) -{ - dAASSERT (L && d && b && n > 0 && nskip >= n); - dSolveL1 (L,b,n,nskip); - dVectorScale (b,d,n); - dSolveL1T (L,b,n,nskip); -} - - -void dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip) -{ - int j,p; - dReal *W1,*W2,W11,W21,alpha1,alpha2,alphanew,gamma1,gamma2,k1,k2,Wp,ell,dee; - dAASSERT (L && d && a && n > 0 && nskip >= n); - - if (n < 2) return; - W1 = (dReal*) ALLOCA (n*sizeof(dReal)); - W2 = (dReal*) ALLOCA (n*sizeof(dReal)); - - W1[0] = 0; - W2[0] = 0; - for (j=1; j<n; j++) W1[j] = W2[j] = a[j] * M_SQRT1_2; - W11 = (REAL(0.5)*a[0]+1)*M_SQRT1_2; - W21 = (REAL(0.5)*a[0]-1)*M_SQRT1_2; - - alpha1=1; - alpha2=1; - - dee = d[0]; - alphanew = alpha1 + (W11*W11)*dee; - dee /= alphanew; - gamma1 = W11 * dee; - dee *= alpha1; - alpha1 = alphanew; - alphanew = alpha2 - (W21*W21)*dee; - dee /= alphanew; - gamma2 = W21 * dee; - alpha2 = alphanew; - k1 = REAL(1.0) - W21*gamma1; - k2 = W21*gamma1*W11 - W21; - for (p=1; p<n; p++) { - Wp = W1[p]; - ell = L[p*nskip]; - W1[p] = Wp - W11*ell; - W2[p] = k1*Wp + k2*ell; - } - - for (j=1; j<n; j++) { - dee = d[j]; - alphanew = alpha1 + (W1[j]*W1[j])*dee; - dee /= alphanew; - gamma1 = W1[j] * dee; - dee *= alpha1; - alpha1 = alphanew; - alphanew = alpha2 - (W2[j]*W2[j])*dee; - dee /= alphanew; - gamma2 = W2[j] * dee; - dee *= alpha2; - d[j] = dee; - alpha2 = alphanew; - - k1 = W1[j]; - k2 = W2[j]; - for (p=j+1; p<n; p++) { - ell = L[p*nskip+j]; - Wp = W1[p] - k1 * ell; - ell += gamma1 * Wp; - W1[p] = Wp; - Wp = W2[p] - k2 * ell; - ell -= gamma2 * Wp; - W2[p] = Wp; - L[p*nskip+j] = ell; - } - } -} - - -// macros for dLDLTRemove() for accessing A - either access the matrix -// directly or access it via row pointers. we are only supposed to reference -// the lower triangle of A (it is symmetric), but indexes i and j come from -// permutation vectors so they are not predictable. so do a test on the -// indexes - this should not slow things down too much, as we don't do this -// in an inner loop. - -#define _GETA(i,j) (A[i][j]) -//#define _GETA(i,j) (A[(i)*nskip+(j)]) -#define GETA(i,j) ((i > j) ? _GETA(i,j) : _GETA(j,i)) - - -void dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d, - int n1, int n2, int r, int nskip) -{ - int i; - dAASSERT(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 && - n1 >= n2 && nskip >= n1); - #ifndef dNODEBUG - for (i=0; i<n2; i++) dIASSERT(p[i] >= 0 && p[i] < n1); - #endif - - if (r==n2-1) { - return; // deleting last row/col is easy - } - else if (r==0) { - dReal *a = (dReal*) ALLOCA (n2 * sizeof(dReal)); - for (i=0; i<n2; i++) a[i] = -GETA(p[i],p[0]); - a[0] += REAL(1.0); - dLDLTAddTL (L,d,a,n2,nskip); - } - else { - dReal *t = (dReal*) ALLOCA (r * sizeof(dReal)); - dReal *a = (dReal*) ALLOCA ((n2-r) * sizeof(dReal)); - for (i=0; i<r; i++) t[i] = L[r*nskip+i] / d[i]; - for (i=0; i<(n2-r); i++) - a[i] = dDot(L+(r+i)*nskip,t,r) - GETA(p[r+i],p[r]); - a[0] += REAL(1.0); - dLDLTAddTL (L + r*nskip+r, d+r, a, n2-r, nskip); - } - - // snip out row/column r from L and d - dRemoveRowCol (L,n2,nskip,r); - if (r < (n2-1)) memmove (d+r,d+r+1,(n2-r-1)*sizeof(dReal)); -} - - -void dRemoveRowCol (dReal *A, int n, int nskip, int r) -{ - int i; - dAASSERT(A && n > 0 && nskip >= n && r >= 0 && r < n); - if (r >= n-1) return; - if (r > 0) { - for (i=0; i<r; i++) - memmove (A+i*nskip+r,A+i*nskip+r+1,(n-r-1)*sizeof(dReal)); - for (i=r; i<(n-1); i++) - memcpy (A+i*nskip,A+i*nskip+nskip,r*sizeof(dReal)); - } - for (i=r; i<(n-1); i++) - memcpy (A+i*nskip+r,A+i*nskip+nskip+r+1,(n-r-1)*sizeof(dReal)); -} diff --git a/extern/ode/dist/ode/src/memory.cpp b/extern/ode/dist/ode/src/memory.cpp deleted file mode 100644 index df61f97375f..00000000000 --- a/extern/ode/dist/ode/src/memory.cpp +++ /dev/null @@ -1,278 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -// if the dDEBUG_ALLOC macro is defined, the following tests are performed in -// the default allocator: -// - size > 0 for alloc and realloc -// - realloc and free operate on uncorrupted blocks -// - realloc and free with the correct sizes -// - on exit, report of block allocation statistics -// - on exit, report of unfreed blocks and if they are corrupted -// the allocator will also call Debug() when it allocates a block with -// sequence number `_d_seqstop' or pointer value `_d_ptrstop'. these variables -// are global and can be set in the debugger. - -#include <ode/config.h> -#include <ode/memory.h> -#include <ode/error.h> - -#ifdef dDEBUG_ALLOC - -// when debugging, this is a header that it put at start of all blocks. -// it contains `padding', which are PADSIZE elements of known value just -// before the user memory starts. another PADSIZE padding elements are put -// at the end of the user memory. the idea is that if the user accidentally -// steps outside the allocated memory, it can hopefully be detected by -// examining the padding elements. - -#define PADSIZE 10 -struct blockHeader { - long pad1; // protective padding - long seq; // sequence number - long size; // data size - long flags; // bit 0 = ignore this block in final report - blockHeader *next,*prev; // next & previous blocks - long pad[PADSIZE]; // protective padding -}; - -// compute the memory block size, including padding. the user memory block is -// rounded up to a multiple of 4 bytes, to get alignment for the padding at -// the end of the block. - -#define BSIZE(size) (((((size)-1) | 3)+1) + sizeof(blockHeader) + \ - PADSIZE * sizeof(long)) - -static blockHeader dummyblock = {0,0,0,0,&dummyblock,&dummyblock, - {0,0,0,0,0,0,0,0,0,0}}; -static blockHeader *firstblock = &dummyblock; -static long num_blocks_alloced = 0; -static long num_bytes_alloced = 0; -static long total_num_blocks_alloced = 0; -static long total_num_bytes_alloced = 0; -static long max_blocks_alloced = 0; -static long max_bytes_alloced = 0; - -long _d_seqstop = 0; -void *_d_ptrstop = 0; - -static int checkBlockOk (void *ptr, int fatal) -{ - if (ptr==0) dDebug (0,"0 passed to checkBlockOk()"); - blockHeader *b = ((blockHeader*) ptr) - 1; - int i,ok = 1; - if (b->pad1 != (long)0xdeadbeef || b->seq < 0 || b->size < 0) ok = 0; - if (ok) { - for (i=0; i<PADSIZE; i++) if (b->pad[i] != (long)0xdeadbeef) ok = 0; - } - if (ok) { - long *endpad = (long*) (((char*)ptr) + (((b->size-1) | 3)+1)); - for (i=0; i<PADSIZE; i++) if (endpad[i] != (long)0xdeadbeef) ok = 0; - } - if (!ok && fatal) - dDebug (0,"corrupted memory block found, ptr=%p, size=%d, " - "seq=%ld", ptr,b->size,b->seq); - return ok; -} - -#endif - - -static dAllocFunction *allocfn = 0; -static dReallocFunction *reallocfn = 0; -static dFreeFunction *freefn = 0; - - - -void dSetAllocHandler (dAllocFunction *fn) -{ - allocfn = fn; -} - - -void dSetReallocHandler (dReallocFunction *fn) -{ - reallocfn = fn; -} - - -void dSetFreeHandler (dFreeFunction *fn) -{ - freefn = fn; -} - - -dAllocFunction *dGetAllocHandler() -{ - return allocfn; -} - - -dReallocFunction *dGetReallocHandler() -{ - return reallocfn; -} - - -dFreeFunction *dGetFreeHandler() -{ - return freefn; -} - - -void * dAlloc (int size) -{ -#ifdef dDEBUG_ALLOC - if (size < 1) dDebug (0,"bad block size to Alloc()"); - - num_blocks_alloced++; - num_bytes_alloced += size; - total_num_blocks_alloced++; - total_num_bytes_alloced += size; - if (num_blocks_alloced > max_blocks_alloced) - max_blocks_alloced = num_blocks_alloced; - if (num_bytes_alloced > max_bytes_alloced) - max_bytes_alloced = num_bytes_alloced; - - if (total_num_blocks_alloced == _d_seqstop) - dDebug (0,"ALLOCATOR TRAP ON SEQUENCE NUMBER %d",_d_seqstop); - long size2 = BSIZE(size); - blockHeader *b = (blockHeader*) malloc (size2); - if (b+1 == _d_ptrstop) - dDebug (0,"ALLOCATOR TRAP ON BLOCK POINTER %p",_d_ptrstop); - for (unsigned int i=0; i < (size2/sizeof(long)); i++) - ((long*)b)[i] = 0xdeadbeef; - b->seq = total_num_blocks_alloced; - b->size = size; - b->flags = 0; - b->next = firstblock->next; - b->prev = firstblock; - firstblock->next->prev = b; - firstblock->next = b; - return b + 1; -#else - if (allocfn) return allocfn (size); else return malloc (size); -#endif -} - - -void * dRealloc (void *ptr, int oldsize, int newsize) -{ -#ifdef dDEBUG_ALLOC - if (ptr==0) dDebug (0,"Realloc() called with ptr==0"); - checkBlockOk (ptr,1); - blockHeader *b = ((blockHeader*) ptr) - 1; - if (b->size != oldsize) - dDebug (0,"Realloc(%p,%d,%d) has bad old size, good size " - "is %ld, seq=%ld",ptr,oldsize,newsize,b->size,b->seq); - void *p = dAlloc (newsize); - blockHeader *newb = ((blockHeader*) p) - 1; - newb->flags = b->flags; - if (oldsize>=1) memcpy (p,ptr,oldsize); - dFree (ptr,oldsize); - return p; -#else - if (reallocfn) return reallocfn (ptr,oldsize,newsize); - else return realloc (ptr,newsize); -#endif -} - - -void dFree (void *ptr, int size) -{ - if (!ptr) return; -#ifdef dDEBUG_ALLOC - checkBlockOk (ptr,1); - blockHeader *b = ((blockHeader*) ptr) - 1; - if (b->size != size) - dDebug (0,"Free(%p,%d) has bad size, good size " - "is %ld, seq=%ld",ptr,size,b->size,b->seq); - num_blocks_alloced--; - num_bytes_alloced -= b->size; - if (num_blocks_alloced < 0 || num_bytes_alloced < 0) - dDebug (0,"Free called too many times"); - - b->next->prev = b->prev; - b->prev->next = b->next; - memset (b,0,BSIZE(b->size)); - - free (b); -#else - if (freefn) freefn (ptr,size); else free (ptr); -#endif -} - - -void dAllocDontReport (void *ptr) -{ -#ifdef dDEBUG_ALLOC - checkBlockOk (ptr,1); - blockHeader *b = ((blockHeader*) ptr) - 1; - b->flags |= 1; -#endif -} - - -#ifdef dDEBUG_ALLOC - -static void printReport() -{ - // subtract the "dont report" blocks from the totals - blockHeader *b = firstblock; - do { - if (b != &dummyblock && (b->flags & 1)) { - num_blocks_alloced--; - num_bytes_alloced -= b->size; - if (!checkBlockOk (b+1,0)) - fprintf (stderr,"\tCORRUPTED: %p, size=%ld, seq=%ld\n",b+1, - b->size,b->seq); - } - b = b->prev; - } - while (b != firstblock); - - fprintf (stderr,"\nALLOCATOR REPORT\n"); - fprintf (stderr,"\tblocks still allocated: %ld\n",num_blocks_alloced); - fprintf (stderr,"\tbytes still allocated: %ld\n",num_bytes_alloced); - fprintf (stderr,"\ttotal blocks allocated: %ld\n",total_num_blocks_alloced); - fprintf (stderr,"\ttotal bytes allocated: %ld\n",total_num_bytes_alloced); - fprintf (stderr,"\tmax blocks allocated: %ld\n",max_blocks_alloced); - fprintf (stderr,"\tmax bytes allocated: %ld\n",max_bytes_alloced); - - b = firstblock; - do { - if (b != &dummyblock && (b->flags & 1)==0) { - int ok = checkBlockOk (b+1,0); - fprintf (stderr,"\tUNFREED: %p, size=%ld, seq=%ld (%s)\n",b+1, - b->size,b->seq, ok ? "ok" : "CORUPTED"); - } - b = b->prev; - } - while (b != firstblock); - fprintf (stderr,"\n"); -} - - -struct dMemoryReportPrinter { - ~dMemoryReportPrinter() { printReport(); } -} _dReportPrinter; - -#endif diff --git a/extern/ode/dist/ode/src/misc.cpp b/extern/ode/dist/ode/src/misc.cpp deleted file mode 100644 index bdc1579d5aa..00000000000 --- a/extern/ode/dist/ode/src/misc.cpp +++ /dev/null @@ -1,147 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#include <ode/config.h> -#include <ode/misc.h> -#include <ode/matrix.h> - -//**************************************************************************** -// random numbers - -static unsigned long seed = 0; - -unsigned long dRand() -{ - seed = (1664525L*seed + 1013904223L) & 0xffffffff; - return seed; -} - - -unsigned long dRandGetSeed() -{ - return seed; -} - - -void dRandSetSeed (unsigned long s) -{ - seed = s; -} - - -int dTestRand() -{ - unsigned long oldseed = seed; - int ret = 1; - seed = 0; - if (dRand() != 0x3c6ef35f || dRand() != 0x47502932 || - dRand() != 0xd1ccf6e9 || dRand() != 0xaaf95334 || - dRand() != 0x6252e503) ret = 0; - seed = oldseed; - return ret; -} - - -int dRandInt (int n) -{ - double a = double(n) / 4294967296.0; - return (int) (double(dRand()) * a); -} - - -dReal dRandReal() -{ - return ((dReal) dRand()) / ((dReal) 0xffffffff); -} - -//**************************************************************************** -// matrix utility stuff - -void dPrintMatrix (dReal *A, int n, int m, char *fmt, FILE *f) -{ - int i,j; - int skip = dPAD(m); - for (i=0; i<n; i++) { - for (j=0; j<m; j++) fprintf (f,fmt,A[i*skip+j]); - fprintf (f,"\n"); - } -} - - -void dMakeRandomVector (dReal *A, int n, dReal range) -{ - int i; - for (i=0; i<n; i++) A[i] = (dRandReal()*REAL(2.0)-REAL(1.0))*range; -} - - -void dMakeRandomMatrix (dReal *A, int n, int m, dReal range) -{ - int i,j; - int skip = dPAD(m); - dSetZero (A,n*skip); - for (i=0; i<n; i++) { - for (j=0; j<m; j++) A[i*skip+j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range; - } -} - - -void dClearUpperTriangle (dReal *A, int n) -{ - int i,j; - int skip = dPAD(n); - for (i=0; i<n; i++) { - for (j=i+1; j<n; j++) A[i*skip+j] = 0; - } -} - - -dReal dMaxDifference (const dReal *A, const dReal *B, int n, int m) -{ - int i,j; - int skip = dPAD(m); - dReal diff,max; - max = 0; - for (i=0; i<n; i++) { - for (j=0; j<m; j++) { - diff = dFabs(A[i*skip+j] - B[i*skip+j]); - if (diff > max) max = diff; - } - } - return max; -} - - -dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n) -{ - int i,j; - int skip = dPAD(n); - dReal diff,max; - max = 0; - for (i=0; i<n; i++) { - for (j=0; j<=i; j++) { - diff = dFabs(A[i*skip+j] - B[i*skip+j]); - if (diff > max) max = diff; - } - } - return max; -} diff --git a/extern/ode/dist/ode/src/objects.h b/extern/ode/dist/ode/src/objects.h deleted file mode 100644 index 4d2c4ca6af7..00000000000 --- a/extern/ode/dist/ode/src/objects.h +++ /dev/null @@ -1,91 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -// object, body, and world structs. - - -#ifndef _ODE_OBJECT_H_ -#define _ODE_OBJECT_H_ - -#include <ode/common.h> -#include <ode/memory.h> -#include <ode/mass.h> -#include "array.h" - - -// some body flags - -enum { - dxBodyFlagFiniteRotation = 1, // use finite rotations - dxBodyFlagFiniteRotationAxis = 2, // use finite rotations only along axis - dxBodyDisabled = 4, // body is disabled - dxBodyNoGravity = 8 // body is not influenced by gravity -}; - - -// base class that does correct object allocation / deallocation - -struct dBase { - void *operator new (size_t size) { return dAlloc (size); } - void operator delete (void *ptr, size_t size) { dFree (ptr,size); } - void *operator new[] (size_t size) { return dAlloc (size); } - void operator delete[] (void *ptr, size_t size) { dFree (ptr,size); } -}; - - -// base class for bodies and joints - -struct dObject : public dBase { - dxWorld *world; // world this object is in - dObject *next; // next object of this type in list - dObject **tome; // pointer to previous object's next ptr - void *userdata; // user settable data - int tag; // used by dynamics algorithms -}; - - -struct dxBody : public dObject { - dxJointNode *firstjoint; // list of attached joints - int flags; // some dxBodyFlagXXX flags - dMass mass; // mass parameters about POR - dMatrix3 invI; // inverse of mass.I - dReal invMass; // 1 / mass.mass - dVector3 pos; // position of POR (point of reference) - dQuaternion q; // orientation quaternion - dMatrix3 R; // rotation matrix, always corresponds to q - dVector3 lvel,avel; // linear and angular velocity of POR - dVector3 facc,tacc; // force and torque accululators - dVector3 finite_rot_axis; // finite rotation axis, unit length or 0=none -}; - - -struct dxWorld : public dBase { - dxBody *firstbody; // body linked list - dxJoint *firstjoint; // joint linked list - int nb,nj; // number of bodies and joints in lists - dVector3 gravity; // gravity vector (m/s/s) - dReal global_erp; // global error reduction parameter - dReal global_cfm; // global costraint force mixing parameter -}; - -#endif - diff --git a/extern/ode/dist/ode/src/obstack.cpp b/extern/ode/dist/ode/src/obstack.cpp deleted file mode 100644 index a6b9d36fbb4..00000000000 --- a/extern/ode/dist/ode/src/obstack.cpp +++ /dev/null @@ -1,130 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#include <ode/common.h> -#include <ode/error.h> -#include <ode/memory.h> -#include "obstack.h" - -//**************************************************************************** -// macros and constants - -#define ROUND_UP_OFFSET_TO_EFFICIENT_SIZE(arena,ofs) \ - ofs = (int) (dEFFICIENT_SIZE( ((intP)(arena)) + ofs ) - ((intP)(arena)) ); - -#define MAX_ALLOC_SIZE \ - ((int)(dOBSTACK_ARENA_SIZE - sizeof (Arena) - EFFICIENT_ALIGNMENT + 1)) - -//**************************************************************************** -// dObStack - -dObStack::dObStack() -{ - first = 0; - last = 0; - current_arena = 0; - current_ofs = 0; -} - - -dObStack::~dObStack() -{ - // free all arenas - Arena *a,*nexta; - a = first; - while (a) { - nexta = a->next; - dFree (a,dOBSTACK_ARENA_SIZE); - a = nexta; - } -} - - -void *dObStack::alloc (int num_bytes) -{ - if (num_bytes > MAX_ALLOC_SIZE) dDebug (0,"num_bytes too large"); - - // allocate or move to a new arena if necessary - if (!first) { - // allocate the first arena if necessary - first = last = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE); - first->next = 0; - first->used = sizeof (Arena); - ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (first,first->used); - } - else { - // we already have one or more arenas, see if a new arena must be used - if ((last->used + num_bytes) > dOBSTACK_ARENA_SIZE) { - if (!last->next) { - last->next = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE); - last->next->next = 0; - } - last = last->next; - last->used = sizeof (Arena); - ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (last,last->used); - } - } - - // allocate an area in the arena - char *c = ((char*) last) + last->used; - last->used += num_bytes; - ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (last,last->used); - return c; -} - - -void dObStack::freeAll() -{ - last = first; - if (first) { - first->used = sizeof(Arena); - ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (first,first->used); - } -} - - -void *dObStack::rewind() -{ - current_arena = first; - current_ofs = sizeof (Arena); - if (current_arena) { - ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs) - return ((char*) current_arena) + current_ofs; - } - else return 0; -} - - -void *dObStack::next (int num_bytes) -{ - // this functions like alloc, except that no new storage is ever allocated - if (!current_arena) return 0; - current_ofs += num_bytes; - ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs); - if (current_ofs >= current_arena->used) { - current_arena = current_arena->next; - if (!current_arena) return 0; - current_ofs = sizeof (Arena); - ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs); - } - return ((char*) current_arena) + current_ofs; -} diff --git a/extern/ode/dist/ode/src/obstack.h b/extern/ode/dist/ode/src/obstack.h deleted file mode 100644 index fa7155dd966..00000000000 --- a/extern/ode/dist/ode/src/obstack.h +++ /dev/null @@ -1,69 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#ifndef _ODE_OBSTACK_H_ -#define _ODE_OBSTACK_H_ - -#include "objects.h" - -// each obstack Arena pointer points to a block of this many bytes -#define dOBSTACK_ARENA_SIZE 16384 - - -struct dObStack : public dBase { - struct Arena { - Arena *next; // next arena in linked list - int used; // total number of bytes used in this arena, counting - }; // this header - - Arena *first; // head of the arena linked list. 0 if no arenas yet - Arena *last; // arena where blocks are currently being allocated - - // used for iterator - Arena *current_arena; - int current_ofs; - - dObStack(); - ~dObStack(); - - void *alloc (int num_bytes); - // allocate a block in the last arena, allocating a new arena if necessary. - // it is a runtime error if num_bytes is larger than the arena size. - - void freeAll(); - // free all blocks in all arenas. this does not deallocate the arenas - // themselves, so future alloc()s will reuse them. - - void *rewind(); - // rewind the obstack iterator, and return the address of the first - // allocated block. return 0 if there are no allocated blocks. - - void *next (int num_bytes); - // return the address of the next allocated block. 'num_bytes' is the size - // of the previous block. this returns null if there are no more arenas. - // the sequence of 'num_bytes' parameters passed to next() during a - // traversal of the list must exactly match the parameters passed to alloc(). -}; - - -#endif - diff --git a/extern/ode/dist/ode/src/ode.cpp b/extern/ode/dist/ode/src/ode.cpp deleted file mode 100644 index 1a14e3292a0..00000000000 --- a/extern/ode/dist/ode/src/ode.cpp +++ /dev/null @@ -1,1341 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -// this source file is mostly concerned with the data structures, not the -// numerics. - -#include "objects.h" -#include <ode/ode.h> -#include "joint.h" -#include <ode/odemath.h> -#include <ode/matrix.h> -#include "step.h" -#include <ode/memory.h> -#include <ode/error.h> - -// misc defines -#define ALLOCA dALLOCA16 - -//**************************************************************************** -// utility - -static inline void initObject (dObject *obj, dxWorld *w) -{ - obj->world = w; - obj->next = 0; - obj->tome = 0; - obj->userdata = 0; - obj->tag = 0; -} - - -// add an object `obj' to the list who's head pointer is pointed to by `first'. - -static inline void addObjectToList (dObject *obj, dObject **first) -{ - obj->next = *first; - obj->tome = first; - if (*first) (*first)->tome = &obj->next; - (*first) = obj; -} - - -// remove the object from the linked list - -static inline void removeObjectFromList (dObject *obj) -{ - if (obj->next) obj->next->tome = obj->tome; - *(obj->tome) = obj->next; - // safeguard - obj->next = 0; - obj->tome = 0; -} - - -// remove the joint from neighbour lists of all connected bodies - -static void removeJointReferencesFromAttachedBodies (dxJoint *j) -{ - for (int i=0; i<2; i++) { - dxBody *body = j->node[i].body; - if (body) { - dxJointNode *n = body->firstjoint; - dxJointNode *last = 0; - while (n) { - if (n->joint == j) { - if (last) last->next = n->next; - else body->firstjoint = n->next; - break; - } - last = n; - n = n->next; - } - } - } - j->node[0].body = 0; - j->node[0].next = 0; - j->node[1].body = 0; - j->node[1].next = 0; -} - -//**************************************************************************** -// island processing - -// this groups all joints and bodies in a world into islands. all objects -// in an island are reachable by going through connected bodies and joints. -// each island can be simulated separately. -// note that joints that are not attached to anything will not be included -// in any island, an so they do not affect the simulation. -// -// this function starts new island from unvisited bodies. however, it will -// never start a new islands from a disabled body. thus islands of disabled -// bodies will not be included in the simulation. disabled bodies are -// re-enabled if they are found to be part of an active island. - -static void processIslands (dxWorld *world, dReal stepsize) -{ - dxBody *b,*bb,**body; - dxJoint *j,**joint; - - // nothing to do if no bodies - if (world->nb <= 0) return; - - // make arrays for body and joint lists (for a single island) to go into - body = (dxBody**) ALLOCA (world->nb * sizeof(dxBody*)); - joint = (dxJoint**) ALLOCA (world->nj * sizeof(dxJoint*)); - int bcount = 0; // number of bodies in `body' - int jcount = 0; // number of joints in `joint' - - // set all body/joint tags to 0 - for (b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0; - for (j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0; - - // allocate a stack of unvisited bodies in the island. the maximum size of - // the stack can be the lesser of the number of bodies or joints, because - // new bodies are only ever added to the stack by going through untagged - // joints. all the bodies in the stack must be tagged! - int stackalloc = (world->nj < world->nb) ? world->nj : world->nb; - dxBody **stack = (dxBody**) ALLOCA (stackalloc * sizeof(dxBody*)); - - for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) { - // get bb = the next enabled, untagged body, and tag it - if (bb->tag || (bb->flags & dxBodyDisabled)) continue; - bb->tag = 1; - - // tag all bodies and joints starting from bb. - int stacksize = 0; - b = bb; - body[0] = bb; - bcount = 1; - jcount = 0; - goto quickstart; - while (stacksize > 0) { - b = stack[--stacksize]; // pop body off stack - body[bcount++] = b; // put body on body list - quickstart: - - // traverse and tag all body's joints, add untagged connected bodies - // to stack - for (dxJointNode *n=b->firstjoint; n; n=n->next) { - if (!n->joint->tag) { - n->joint->tag = 1; - joint[jcount++] = n->joint; - if (n->body && !n->body->tag) { - n->body->tag = 1; - stack[stacksize++] = n->body; - } - } - } - dIASSERT(stacksize <= world->nb); - dIASSERT(stacksize <= world->nj); - } - - // now do something with body and joint lists - dInternalStepIsland (world,body,bcount,joint,jcount,stepsize); - - // what we've just done may have altered the body/joint tag values. - // we must make sure that these tags are nonzero. - // also make sure all bodies are in the enabled state. - int i; - for (i=0; i<bcount; i++) { - body[i]->tag = 1; - body[i]->flags &= ~dxBodyDisabled; - } - for (i=0; i<jcount; i++) joint[i]->tag = 1; - } - - // if debugging, check that all objects (except for disabled bodies, - // unconnected joints, and joints that are connected to disabled bodies) - // were tagged. -# ifndef dNODEBUG - for (b=world->firstbody; b; b=(dxBody*)b->next) { - if (b->flags & dxBodyDisabled) { - if (b->tag) dDebug (0,"disabled body tagged"); - } - else { - if (!b->tag) dDebug (0,"enabled body not tagged"); - } - } - for (j=world->firstjoint; j; j=(dxJoint*)j->next) { - if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled)==0) || - (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled)==0)) { - if (!j->tag) dDebug (0,"attached enabled joint not tagged"); - } - else { - if (j->tag) dDebug (0,"unattached or disabled joint tagged"); - } - } -# endif -} - -//**************************************************************************** -// debugging - -// see if an object list loops on itself (if so, it's bad). - -static int listHasLoops (dObject *first) -{ - if (first==0 || first->next==0) return 0; - dObject *a=first,*b=first->next; - int skip=0; - while (b) { - if (a==b) return 1; - b = b->next; - if (skip) a = a->next; - skip ^= 1; - } - return 0; -} - - -// check the validity of the world data structures - -static void checkWorld (dxWorld *w) -{ - dxBody *b; - dxJoint *j; - - // check there are no loops - if (listHasLoops (w->firstbody)) dDebug (0,"body list has loops"); - if (listHasLoops (w->firstjoint)) dDebug (0,"joint list has loops"); - - // check lists are well formed (check `tome' pointers) - for (b=w->firstbody; b; b=(dxBody*)b->next) { - if (b->next && b->next->tome != &b->next) - dDebug (0,"bad tome pointer in body list"); - } - for (j=w->firstjoint; j; j=(dxJoint*)j->next) { - if (j->next && j->next->tome != &j->next) - dDebug (0,"bad tome pointer in joint list"); - } - - // check counts - int n = 0; - for (b=w->firstbody; b; b=(dxBody*)b->next) n++; - if (w->nb != n) dDebug (0,"body count incorrect"); - n = 0; - for (j=w->firstjoint; j; j=(dxJoint*)j->next) n++; - if (w->nj != n) dDebug (0,"joint count incorrect"); - - // set all tag values to a known value - static int count = 0; - count++; - for (b=w->firstbody; b; b=(dxBody*)b->next) b->tag = count; - for (j=w->firstjoint; j; j=(dxJoint*)j->next) j->tag = count; - - // check all body/joint world pointers are ok - for (b=w->firstbody; b; b=(dxBody*)b->next) if (b->world != w) - dDebug (0,"bad world pointer in body list"); - for (j=w->firstjoint; j; j=(dxJoint*)j->next) if (j->world != w) - dDebug (0,"bad world pointer in joint list"); - - /* - // check for half-connected joints - actually now these are valid - for (j=w->firstjoint; j; j=(dxJoint*)j->next) { - if (j->node[0].body || j->node[1].body) { - if (!(j->node[0].body && j->node[1].body)) - dDebug (0,"half connected joint found"); - } - } - */ - - // check that every joint node appears in the joint lists of both bodies it - // attaches - for (j=w->firstjoint; j; j=(dxJoint*)j->next) { - for (int i=0; i<2; i++) { - if (j->node[i].body) { - int ok = 0; - for (dxJointNode *n=j->node[i].body->firstjoint; n; n=n->next) { - if (n->joint == j) ok = 1; - } - if (ok==0) dDebug (0,"joint not in joint list of attached body"); - } - } - } - - // check all body joint lists (correct body ptrs) - for (b=w->firstbody; b; b=(dxBody*)b->next) { - for (dxJointNode *n=b->firstjoint; n; n=n->next) { - if (&n->joint->node[0] == n) { - if (n->joint->node[1].body != b) - dDebug (0,"bad body pointer in joint node of body list (1)"); - } - else { - if (n->joint->node[0].body != b) - dDebug (0,"bad body pointer in joint node of body list (2)"); - } - if (n->joint->tag != count) dDebug (0,"bad joint node pointer in body"); - } - } - - // check all body pointers in joints, check they are distinct - for (j=w->firstjoint; j; j=(dxJoint*)j->next) { - if (j->node[0].body && (j->node[0].body == j->node[1].body)) - dDebug (0,"non-distinct body pointers in joint"); - if ((j->node[0].body && j->node[0].body->tag != count) || - (j->node[1].body && j->node[1].body->tag != count)) - dDebug (0,"bad body pointer in joint"); - } -} - - -void dWorldCheck (dxWorld *w) -{ - checkWorld (w); -} - -//**************************************************************************** -// body - -dxBody *dBodyCreate (dxWorld *w) -{ - dAASSERT (w); - dxBody *b = new dxBody; - initObject (b,w); - b->firstjoint = 0; - b->flags = 0; - dMassSetParameters (&b->mass,1,0,0,0,1,1,1,0,0,0); - dSetZero (b->invI,4*3); - b->invI[0] = 1; - b->invI[5] = 1; - b->invI[10] = 1; - b->invMass = 1; - dSetZero (b->pos,4); - dSetZero (b->q,4); - b->q[0] = 1; - dRSetIdentity (b->R); - dSetZero (b->lvel,4); - dSetZero (b->avel,4); - dSetZero (b->facc,4); - dSetZero (b->tacc,4); - dSetZero (b->finite_rot_axis,4); - addObjectToList (b,(dObject **) &w->firstbody); - w->nb++; - return b; -} - - -void dBodyDestroy (dxBody *b) -{ - dAASSERT (b); - - // detach all neighbouring joints, then delete this body. - dxJointNode *n = b->firstjoint; - while (n) { - // sneaky trick to speed up removal of joint references (black magic) - n->joint->node[(n == n->joint->node)].body = 0; - - dxJointNode *next = n->next; - n->next = 0; - removeJointReferencesFromAttachedBodies (n->joint); - n = next; - } - removeObjectFromList (b); - b->world->nb--; - delete b; -} - - -void dBodySetData (dBodyID b, void *data) -{ - dAASSERT (b); - b->userdata = data; -} - - -void *dBodyGetData (dBodyID b) -{ - dAASSERT (b); - return b->userdata; -} - - -void dBodySetPosition (dBodyID b, dReal x, dReal y, dReal z) -{ - dAASSERT (b); - b->pos[0] = x; - b->pos[1] = y; - b->pos[2] = z; -} - - -void dBodySetRotation (dBodyID b, const dMatrix3 R) -{ - dAASSERT (b && R); - dQuaternion q; - dRtoQ (R,q); - dNormalize4 (q); - b->q[0] = q[0]; - b->q[1] = q[1]; - b->q[2] = q[2]; - b->q[3] = q[3]; - dQtoR (b->q,b->R); -} - - -void dBodySetQuaternion (dBodyID b, const dQuaternion q) -{ - dAASSERT (b && q); - b->q[0] = q[0]; - b->q[1] = q[1]; - b->q[2] = q[2]; - b->q[3] = q[3]; - dNormalize4 (b->q); - dQtoR (b->q,b->R); -} - - -void dBodySetLinearVel (dBodyID b, dReal x, dReal y, dReal z) -{ - dAASSERT (b); - b->lvel[0] = x; - b->lvel[1] = y; - b->lvel[2] = z; -} - - -void dBodySetAngularVel (dBodyID b, dReal x, dReal y, dReal z) -{ - dAASSERT (b); - b->avel[0] = x; - b->avel[1] = y; - b->avel[2] = z; -} - - -const dReal * dBodyGetPosition (dBodyID b) -{ - dAASSERT (b); - return b->pos; -} - - -const dReal * dBodyGetRotation (dBodyID b) -{ - dAASSERT (b); - return b->R; -} - - -const dReal * dBodyGetQuaternion (dBodyID b) -{ - dAASSERT (b); - return b->q; -} - - -const dReal * dBodyGetLinearVel (dBodyID b) -{ - dAASSERT (b); - return b->lvel; -} - - -const dReal * dBodyGetAngularVel (dBodyID b) -{ - dAASSERT (b); - return b->avel; -} - - -void dBodySetMass (dBodyID b, const dMass *mass) -{ - dAASSERT (b && mass); - memcpy (&b->mass,mass,sizeof(dMass)); - if (dInvertPDMatrix (b->mass.I,b->invI,3)==0) { - dDEBUGMSG ("inertia must be positive definite"); - dRSetIdentity (b->invI); - } - b->invMass = dRecip(b->mass.mass); -} - - -void dBodyGetMass (dBodyID b, dMass *mass) -{ - dAASSERT (b && mass); - memcpy (mass,&b->mass,sizeof(dMass)); -} - - -void dBodyAddForce (dBodyID b, dReal fx, dReal fy, dReal fz) -{ - dAASSERT (b); - b->facc[0] += fx; - b->facc[1] += fy; - b->facc[2] += fz; -} - - -void dBodyAddTorque (dBodyID b, dReal fx, dReal fy, dReal fz) -{ - dAASSERT (b); - b->tacc[0] += fx; - b->tacc[1] += fy; - b->tacc[2] += fz; -} - - -void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz) -{ - dAASSERT (b); - dVector3 t1,t2; - t1[0] = fx; - t1[1] = fy; - t1[2] = fz; - t1[3] = 0; - dMULTIPLY0_331 (t2,b->R,t1); - b->facc[0] += t2[0]; - b->facc[1] += t2[1]; - b->facc[2] += t2[2]; -} - - -void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz) -{ - dAASSERT (b); - dVector3 t1,t2; - t1[0] = fx; - t1[1] = fy; - t1[2] = fz; - t1[3] = 0; - dMULTIPLY0_331 (t2,b->R,t1); - b->tacc[0] += t2[0]; - b->tacc[1] += t2[1]; - b->tacc[2] += t2[2]; -} - - -void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz, - dReal px, dReal py, dReal pz) -{ - dAASSERT (b); - b->facc[0] += fx; - b->facc[1] += fy; - b->facc[2] += fz; - dVector3 f,q; - f[0] = fx; - f[1] = fy; - f[2] = fz; - q[0] = px - b->pos[0]; - q[1] = py - b->pos[1]; - q[2] = pz - b->pos[2]; - dCROSS (b->tacc,+=,q,f); -} - - -void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz, - dReal px, dReal py, dReal pz) -{ - dAASSERT (b); - dVector3 prel,f,p; - f[0] = fx; - f[1] = fy; - f[2] = fz; - f[3] = 0; - prel[0] = px; - prel[1] = py; - prel[2] = pz; - prel[3] = 0; - dMULTIPLY0_331 (p,b->R,prel); - b->facc[0] += f[0]; - b->facc[1] += f[1]; - b->facc[2] += f[2]; - dCROSS (b->tacc,+=,p,f); -} - - -void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz, - dReal px, dReal py, dReal pz) -{ - dAASSERT (b); - dVector3 frel,f; - frel[0] = fx; - frel[1] = fy; - frel[2] = fz; - frel[3] = 0; - dMULTIPLY0_331 (f,b->R,frel); - b->facc[0] += f[0]; - b->facc[1] += f[1]; - b->facc[2] += f[2]; - dVector3 q; - q[0] = px - b->pos[0]; - q[1] = py - b->pos[1]; - q[2] = pz - b->pos[2]; - dCROSS (b->tacc,+=,q,f); -} - - -void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz, - dReal px, dReal py, dReal pz) -{ - dAASSERT (b); - dVector3 frel,prel,f,p; - frel[0] = fx; - frel[1] = fy; - frel[2] = fz; - frel[3] = 0; - prel[0] = px; - prel[1] = py; - prel[2] = pz; - prel[3] = 0; - dMULTIPLY0_331 (f,b->R,frel); - dMULTIPLY0_331 (p,b->R,prel); - b->facc[0] += f[0]; - b->facc[1] += f[1]; - b->facc[2] += f[2]; - dCROSS (b->tacc,+=,p,f); -} - - -const dReal * dBodyGetForce (dBodyID b) -{ - dAASSERT (b); - return b->facc; -} - - -const dReal * dBodyGetTorque (dBodyID b) -{ - dAASSERT (b); - return b->tacc; -} - - -void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z) -{ - dAASSERT (b); - b->facc[0] = x; - b->facc[1] = y; - b->facc[2] = z; -} - - -void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z) -{ - dAASSERT (b); - b->tacc[0] = x; - b->tacc[1] = y; - b->tacc[2] = z; -} - - -void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz, - dVector3 result) -{ - dAASSERT (b); - dVector3 prel,p; - prel[0] = px; - prel[1] = py; - prel[2] = pz; - prel[3] = 0; - dMULTIPLY0_331 (p,b->R,prel); - result[0] = p[0] + b->pos[0]; - result[1] = p[1] + b->pos[1]; - result[2] = p[2] + b->pos[2]; -} - - -void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz, - dVector3 result) -{ - dAASSERT (b); - dVector3 prel,p; - prel[0] = px; - prel[1] = py; - prel[2] = pz; - prel[3] = 0; - dMULTIPLY0_331 (p,b->R,prel); - result[0] = b->lvel[0]; - result[1] = b->lvel[1]; - result[2] = b->lvel[2]; - dCROSS (result,+=,b->avel,p); -} - - -void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz, - dVector3 result) -{ - dAASSERT (b); - dVector3 p; - p[0] = px - b->pos[0]; - p[1] = py - b->pos[1]; - p[2] = pz - b->pos[2]; - p[3] = 0; - result[0] = b->lvel[0]; - result[1] = b->lvel[1]; - result[2] = b->lvel[2]; - dCROSS (result,+=,b->avel,p); -} - - -void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz, - dVector3 result) -{ - dAASSERT (b); - dVector3 prel; - prel[0] = px - b->pos[0]; - prel[1] = py - b->pos[1]; - prel[2] = pz - b->pos[2]; - prel[3] = 0; - dMULTIPLY1_331 (result,b->R,prel); -} - - -void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz, - dVector3 result) -{ - dAASSERT (b); - dVector3 p; - p[0] = px; - p[1] = py; - p[2] = pz; - p[3] = 0; - dMULTIPLY0_331 (result,b->R,p); -} - - -void dBodyVectorFromWorld (dBodyID b, dReal px, dReal py, dReal pz, - dVector3 result) -{ - dAASSERT (b); - dVector3 p; - p[0] = px; - p[1] = py; - p[2] = pz; - p[3] = 0; - dMULTIPLY1_331 (result,b->R,p); -} - - -void dBodySetFiniteRotationMode (dBodyID b, int mode) -{ - dAASSERT (b); - b->flags &= ~(dxBodyFlagFiniteRotation | dxBodyFlagFiniteRotationAxis); - if (mode) { - b->flags |= dxBodyFlagFiniteRotation; - if (b->finite_rot_axis[0] != 0 || b->finite_rot_axis[1] != 0 || - b->finite_rot_axis[2] != 0) { - b->flags |= dxBodyFlagFiniteRotationAxis; - } - } -} - - -void dBodySetFiniteRotationAxis (dBodyID b, dReal x, dReal y, dReal z) -{ - dAASSERT (b); - b->finite_rot_axis[0] = x; - b->finite_rot_axis[1] = y; - b->finite_rot_axis[2] = z; - if (x != 0 || y != 0 || z != 0) { - dNormalize3 (b->finite_rot_axis); - b->flags |= dxBodyFlagFiniteRotationAxis; - } - else { - b->flags &= ~dxBodyFlagFiniteRotationAxis; - } -} - - -int dBodyGetFiniteRotationMode (dBodyID b) -{ - dAASSERT (b); - return ((b->flags & dxBodyFlagFiniteRotation) != 0); -} - - -void dBodyGetFiniteRotationAxis (dBodyID b, dVector3 result) -{ - dAASSERT (b); - result[0] = b->finite_rot_axis[0]; - result[1] = b->finite_rot_axis[1]; - result[2] = b->finite_rot_axis[2]; -} - - -int dBodyGetNumJoints (dBodyID b) -{ - dAASSERT (b); - int count=0; - for (dxJointNode *n=b->firstjoint; n; n=n->next, count++); - return count; -} - - -dJointID dBodyGetJoint (dBodyID b, int index) -{ - dAASSERT (b); - int i=0; - for (dxJointNode *n=b->firstjoint; n; n=n->next, i++) { - if (i == index) return n->joint; - } - return 0; -} - - -void dBodyEnable (dBodyID b) -{ - dAASSERT (b); - b->flags &= ~dxBodyDisabled; -} - - -void dBodyDisable (dBodyID b) -{ - dAASSERT (b); - b->flags |= dxBodyDisabled; -} - - -int dBodyIsEnabled (dBodyID b) -{ - dAASSERT (b); - return ((b->flags & dxBodyDisabled) == 0); -} - - -void dBodySetGravityMode (dBodyID b, int mode) -{ - dAASSERT (b); - if (mode) b->flags &= ~dxBodyNoGravity; - else b->flags |= dxBodyNoGravity; -} - - -int dBodyGetGravityMode (dBodyID b) -{ - dAASSERT (b); - return ((b->flags & dxBodyNoGravity) == 0); -} - -//**************************************************************************** -// joints - -static void dJointInit (dxWorld *w, dxJoint *j) -{ - dIASSERT (w && j); - initObject (j,w); - j->vtable = 0; - j->flags = 0; - j->node[0].joint = j; - j->node[0].body = 0; - j->node[0].next = 0; - j->node[1].joint = j; - j->node[1].body = 0; - j->node[1].next = 0; - addObjectToList (j,(dObject **) &w->firstjoint); - w->nj++; -} - - -static dxJoint *createJoint (dWorldID w, dJointGroupID group, - dxJoint::Vtable *vtable) -{ - dIASSERT (w && vtable); - dxJoint *j; - if (group) { - j = (dxJoint*) group->stack.alloc (vtable->size); - group->num++; - } - else j = (dxJoint*) dAlloc (vtable->size); - dJointInit (w,j); - j->vtable = vtable; - if (group) j->flags |= dJOINT_INGROUP; - if (vtable->init) vtable->init (j); - j->feedback = 0; - return j; -} - - -dxJoint * dJointCreateBall (dWorldID w, dJointGroupID group) -{ - dAASSERT (w); - return createJoint (w,group,&__dball_vtable); -} - - -dxJoint * dJointCreateHinge (dWorldID w, dJointGroupID group) -{ - dAASSERT (w); - return createJoint (w,group,&__dhinge_vtable); -} - - -dxJoint * dJointCreateSlider (dWorldID w, dJointGroupID group) -{ - dAASSERT (w); - return createJoint (w,group,&__dslider_vtable); -} - - -dxJoint * dJointCreateContact (dWorldID w, dJointGroupID group, - const dContact *c) -{ - dAASSERT (w && c); - dxJointContact *j = (dxJointContact *) - createJoint (w,group,&__dcontact_vtable); - j->contact = *c; - return j; -} - - -dxJoint * dJointCreateHinge2 (dWorldID w, dJointGroupID group) -{ - dAASSERT (w); - return createJoint (w,group,&__dhinge2_vtable); -} - - -dxJoint * dJointCreateUniversal (dWorldID w, dJointGroupID group) -{ - dAASSERT (w); - return createJoint (w,group,&__duniversal_vtable); -} - - -dxJoint * dJointCreateFixed (dWorldID w, dJointGroupID group) -{ - dAASSERT (w); - return createJoint (w,group,&__dfixed_vtable); -} - - -dxJoint * dJointCreateNull (dWorldID w, dJointGroupID group) -{ - dAASSERT (w); - return createJoint (w,group,&__dnull_vtable); -} - - -dxJoint * dJointCreateAMotor (dWorldID w, dJointGroupID group) -{ - dAASSERT (w); - return createJoint (w,group,&__damotor_vtable); -} - - -void dJointDestroy (dxJoint *j) -{ - dAASSERT (j); - if (j->flags & dJOINT_INGROUP) return; - removeJointReferencesFromAttachedBodies (j); - removeObjectFromList (j); - j->world->nj--; - dFree (j,j->vtable->size); -} - - -dJointGroupID dJointGroupCreate (int max_size) -{ - // not any more ... dUASSERT (max_size > 0,"max size must be > 0"); - dxJointGroup *group = new dxJointGroup; - group->num = 0; - return group; -} - - -void dJointGroupDestroy (dJointGroupID group) -{ - dAASSERT (group); - dJointGroupEmpty (group); - delete group; -} - - -void dJointGroupEmpty (dJointGroupID group) -{ - // the joints in this group are detached starting from the most recently - // added (at the top of the stack). this helps ensure that the various - // linked lists are not traversed too much, as the joints will hopefully - // be at the start of those lists. - // if any group joints have their world pointer set to 0, their world was - // previously destroyed. no special handling is required for these joints. - - dAASSERT (group); - int i; - dxJoint **jlist = (dxJoint**) ALLOCA (group->num * sizeof(dxJoint*)); - dxJoint *j = (dxJoint*) group->stack.rewind(); - for (i=0; i < group->num; i++) { - jlist[i] = j; - j = (dxJoint*) (group->stack.next (j->vtable->size)); - } - for (i=group->num-1; i >= 0; i--) { - if (jlist[i]->world) { - removeJointReferencesFromAttachedBodies (jlist[i]); - removeObjectFromList (jlist[i]); - jlist[i]->world->nj--; - } - } - group->num = 0; - group->stack.freeAll(); -} - - -void dJointAttach (dxJoint *joint, dxBody *body1, dxBody *body2) -{ - // check arguments - dUASSERT (joint,"bad joint argument"); - dUASSERT (body1 == 0 || body1 != body2,"can't have body1==body2"); - dxWorld *world = joint->world; - dUASSERT ( (!body1 || body1->world == world) && - (!body2 || body2->world == world), - "joint and bodies must be in same world"); - - // check if the joint can not be attached to just one body - dUASSERT (!((joint->flags & dJOINT_TWOBODIES) && - ((body1 != 0) ^ (body2 != 0))), - "joint can not be attached to just one body"); - - // remove any existing body attachments - if (joint->node[0].body || joint->node[1].body) { - removeJointReferencesFromAttachedBodies (joint); - } - - // if a body is zero, make sure that it is body2, so 0 --> node[1].body - if (body1==0) { - body1 = body2; - body2 = 0; - joint->flags &= (~dJOINT_REVERSE); - } - else { - joint->flags |= dJOINT_REVERSE; - } - - // attach to new bodies - joint->node[0].body = body1; - joint->node[1].body = body2; - if (body1) { - joint->node[1].next = body1->firstjoint; - body1->firstjoint = &joint->node[1]; - } - else joint->node[1].next = 0; - if (body2) { - joint->node[0].next = body2->firstjoint; - body2->firstjoint = &joint->node[0]; - } - else { - joint->node[0].next = 0; - } -} - - -void dJointSetData (dxJoint *joint, void *data) -{ - dAASSERT (joint); - joint->userdata = data; -} - - -void *dJointGetData (dxJoint *joint) -{ - dAASSERT (joint); - return joint->userdata; -} - - -int dJointGetType (dxJoint *joint) -{ - dAASSERT (joint); - return joint->vtable->typenum; -} - - -dBodyID dJointGetBody (dxJoint *joint, int index) -{ - dAASSERT (joint); - if (index >= 0 && index < 2) return joint->node[index].body; - else return 0; -} - - -void dJointSetFeedback (dxJoint *joint, dJointFeedback *f) -{ - dAASSERT (joint); - joint->feedback = f; -} - - -dJointFeedback *dJointGetFeedback (dxJoint *joint) -{ - dAASSERT (joint); - return joint->feedback; -} - - -int dAreConnected (dBodyID b1, dBodyID b2) -{ - dAASSERT (b1 && b2); - // look through b1's neighbour list for b2 - for (dxJointNode *n=b1->firstjoint; n; n=n->next) { - if (n->body == b2) return 1; - } - return 0; -} - -//**************************************************************************** -// world - -dxWorld * dWorldCreate() -{ - dxWorld *w = new dxWorld; - w->firstbody = 0; - w->firstjoint = 0; - w->nb = 0; - w->nj = 0; - dSetZero (w->gravity,4); - w->global_erp = REAL(0.2); -#if defined(dSINGLE) - w->global_cfm = 1e-5f; -#elif defined(dDOUBLE) - w->global_cfm = 1e-10; -#else - #error dSINGLE or dDOUBLE must be defined -#endif - return w; -} - - -void dWorldDestroy (dxWorld *w) -{ - // delete all bodies and joints - dAASSERT (w); - dxBody *nextb, *b = w->firstbody; - while (b) { - nextb = (dxBody*) b->next; - delete b; - b = nextb; - } - dxJoint *nextj, *j = w->firstjoint; - while (j) { - nextj = (dxJoint*)j->next; - if (j->flags & dJOINT_INGROUP) { - // the joint is part of a group, so "deactivate" it instead - j->world = 0; - j->node[0].body = 0; - j->node[0].next = 0; - j->node[1].body = 0; - j->node[1].next = 0; - dMessage (0,"warning: destroying world containing grouped joints"); - } - else { - dFree (j,j->vtable->size); - } - j = nextj; - } - delete w; -} - - -void dWorldSetGravity (dWorldID w, dReal x, dReal y, dReal z) -{ - dAASSERT (w); - w->gravity[0] = x; - w->gravity[1] = y; - w->gravity[2] = z; -} - - -void dWorldGetGravity (dWorldID w, dVector3 g) -{ - dAASSERT (w); - g[0] = w->gravity[0]; - g[1] = w->gravity[1]; - g[2] = w->gravity[2]; -} - - -void dWorldSetERP (dWorldID w, dReal erp) -{ - dAASSERT (w); - w->global_erp = erp; -} - - -dReal dWorldGetERP (dWorldID w) -{ - dAASSERT (w); - return w->global_erp; -} - - -void dWorldSetCFM (dWorldID w, dReal cfm) -{ - dAASSERT (w); - w->global_cfm = cfm; -} - - -dReal dWorldGetCFM (dWorldID w) -{ - dAASSERT (w); - return w->global_cfm; -} - - -void dWorldStep (dWorldID w, dReal stepsize) -{ - dUASSERT (w,"bad world argument"); - dUASSERT (stepsize > 0,"stepsize must be > 0"); - processIslands (w,stepsize); -} - - -void dWorldImpulseToForce (dWorldID w, dReal stepsize, - dReal ix, dReal iy, dReal iz, - dVector3 force) -{ - dAASSERT (w); - stepsize = dRecip(stepsize); - force[0] = stepsize * ix; - force[1] = stepsize * iy; - force[2] = stepsize * iz; - // @@@ force[3] = 0; -} - -//**************************************************************************** -// testing - -#define NUM 100 - -#define DO(x) - - -extern "C" void dTestDataStructures() -{ - int i; - DO(printf ("testDynamicsStuff()\n")); - - dBodyID body [NUM]; - int nb = 0; - dJointID joint [NUM]; - int nj = 0; - - for (i=0; i<NUM; i++) body[i] = 0; - for (i=0; i<NUM; i++) joint[i] = 0; - - DO(printf ("creating world\n")); - dWorldID w = dWorldCreate(); - checkWorld (w); - - for (;;) { - if (nb < NUM && dRandReal() > 0.5) { - DO(printf ("creating body\n")); - body[nb] = dBodyCreate (w); - DO(printf ("\t--> %p\n",body[nb])); - nb++; - checkWorld (w); - DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); - } - if (nj < NUM && nb > 2 && dRandReal() > 0.5) { - dBodyID b1 = body [dRand() % nb]; - dBodyID b2 = body [dRand() % nb]; - if (b1 != b2) { - DO(printf ("creating joint, attaching to %p,%p\n",b1,b2)); - joint[nj] = dJointCreateBall (w,0); - DO(printf ("\t-->%p\n",joint[nj])); - checkWorld (w); - dJointAttach (joint[nj],b1,b2); - nj++; - checkWorld (w); - DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); - } - } - if (nj > 0 && nb > 2 && dRandReal() > 0.5) { - dBodyID b1 = body [dRand() % nb]; - dBodyID b2 = body [dRand() % nb]; - if (b1 != b2) { - int k = dRand() % nj; - DO(printf ("reattaching joint %p\n",joint[k])); - dJointAttach (joint[k],b1,b2); - checkWorld (w); - DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); - } - } - if (nb > 0 && dRandReal() > 0.5) { - int k = dRand() % nb; - DO(printf ("destroying body %p\n",body[k])); - dBodyDestroy (body[k]); - checkWorld (w); - for (; k < (NUM-1); k++) body[k] = body[k+1]; - nb--; - DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); - } - if (nj > 0 && dRandReal() > 0.5) { - int k = dRand() % nj; - DO(printf ("destroying joint %p\n",joint[k])); - dJointDestroy (joint[k]); - checkWorld (w); - for (; k < (NUM-1); k++) joint[k] = joint[k+1]; - nj--; - DO(printf ("%d BODIES, %d JOINTS\n",nb,nj)); - } - } - - /* - printf ("creating world\n"); - dWorldID w = dWorldCreate(); - checkWorld (w); - printf ("creating body\n"); - dBodyID b1 = dBodyCreate (w); - checkWorld (w); - printf ("creating body\n"); - dBodyID b2 = dBodyCreate (w); - checkWorld (w); - printf ("creating joint\n"); - dJointID j = dJointCreateBall (w); - checkWorld (w); - printf ("attaching joint\n"); - dJointAttach (j,b1,b2); - checkWorld (w); - printf ("destroying joint\n"); - dJointDestroy (j); - checkWorld (w); - printf ("destroying body\n"); - dBodyDestroy (b1); - checkWorld (w); - printf ("destroying body\n"); - dBodyDestroy (b2); - checkWorld (w); - printf ("destroying world\n"); - dWorldDestroy (w); - */ -} diff --git a/extern/ode/dist/ode/src/odemath.cpp b/extern/ode/dist/ode/src/odemath.cpp deleted file mode 100644 index cd6dbc855fe..00000000000 --- a/extern/ode/dist/ode/src/odemath.cpp +++ /dev/null @@ -1,173 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#define SHARED_CONFIG_H_INCLUDED_FROM_DEFINING_FILE 1 -#include <ode/common.h> -#include <ode/odemath.h> - - -// get some math functions under windows -#ifdef WIN32 -#include <float.h> -#ifndef CYGWIN // added by andy for cygwin -#define copysign(a,b) ((dReal)_copysign(a,b)) -#endif // added by andy for cygwin -#endif - - -// infinity declaration - -#ifdef DINFINITY_DECL -DINFINITY_DECL -#endif - - -// this may be called for vectors `a' with extremely small magnitude, for -// example the result of a cross product on two nearly perpendicular vectors. -// we must be robust to these small vectors. to prevent numerical error, -// first find the component a[i] with the largest magnitude and then scale -// all the components by 1/a[i]. then we can compute the length of `a' and -// scale the components by 1/l. this has been verified to work with vectors -// containing the smallest representable numbers. - -void dNormalize3 (dVector3 a) -{ - dReal a0,a1,a2,aa0,aa1,aa2,l; - dAASSERT (a); - a0 = a[0]; - a1 = a[1]; - a2 = a[2]; - aa0 = dFabs(a0); - aa1 = dFabs(a1); - aa2 = dFabs(a2); - if (aa1 > aa0) { - if (aa2 > aa1) { - goto aa2_largest; - } - else { // aa1 is largest - a0 /= aa1; - a2 /= aa1; - l = dRecipSqrt (a0*a0 + a2*a2 + 1); - a[0] = a0*l; - a[1] = copysign(l,a1); - a[2] = a2*l; - } - } - else { - if (aa2 > aa0) { - aa2_largest: // aa2 is largest - a0 /= aa2; - a1 /= aa2; - l = dRecipSqrt (a0*a0 + a1*a1 + 1); - a[0] = a0*l; - a[1] = a1*l; - a[2] = copysign(l,a2); - } - else { // aa0 is largest - if (aa0 <= 0) { - dDEBUGMSG ("vector has zero size"); - a[0] = 1; // if all a's are zero, this is where we'll end up. - a[1] = 0; // return a default unit length vector. - a[2] = 0; - return; - } - a1 /= aa0; - a2 /= aa0; - l = dRecipSqrt (a1*a1 + a2*a2 + 1); - a[0] = copysign(l,a0); - a[1] = a1*l; - a[2] = a2*l; - } - } -} - - -/* OLD VERSION */ -/* -void dNormalize3 (dVector3 a) -{ - dASSERT (a); - dReal l = dDOT(a,a); - if (l > 0) { - l = dRecipSqrt(l); - a[0] *= l; - a[1] *= l; - a[2] *= l; - } - else { - a[0] = 1; - a[1] = 0; - a[2] = 0; - } -} -*/ - - -void dNormalize4 (dVector4 a) -{ - dAASSERT (a); - dReal l = dDOT(a,a)+a[3]*a[3]; - if (l > 0) { - l = dRecipSqrt(l); - a[0] *= l; - a[1] *= l; - a[2] *= l; - a[3] *= l; - } - else { - dDEBUGMSG ("vector has zero size"); - a[0] = 1; - a[1] = 0; - a[2] = 0; - a[3] = 0; - } -} - - -void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q) -{ - dAASSERT (n && p && q); - if (dFabs(n[2]) > M_SQRT1_2) { - // choose p in y-z plane - dReal a = n[1]*n[1] + n[2]*n[2]; - dReal k = dRecipSqrt (a); - p[0] = 0; - p[1] = -n[2]*k; - p[2] = n[1]*k; - // set q = n x p - q[0] = a*k; - q[1] = -n[0]*p[2]; - q[2] = n[0]*p[1]; - } - else { - // choose p in x-y plane - dReal a = n[0]*n[0] + n[1]*n[1]; - dReal k = dRecipSqrt (a); - p[0] = -n[1]*k; - p[1] = n[0]*k; - p[2] = 0; - // set q = n x p - q[0] = -n[2]*p[1]; - q[1] = n[2]*p[0]; - q[2] = a*k; - } -} diff --git a/extern/ode/dist/ode/src/rotation.cpp b/extern/ode/dist/ode/src/rotation.cpp deleted file mode 100644 index 22b9fb13820..00000000000 --- a/extern/ode/dist/ode/src/rotation.cpp +++ /dev/null @@ -1,283 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -/* - -quaternions have the format: (s,vx,vy,vz) where (vx,vy,vz) is the -"rotation axis" and s is the "rotation angle". - -*/ - -#include <ode/rotation.h> - - -#define _R(i,j) R[(i)*4+(j)] - -#define SET_3x3_IDENTITY \ - _R(0,0) = REAL(1.0); \ - _R(0,1) = REAL(0.0); \ - _R(0,2) = REAL(0.0); \ - _R(0,3) = REAL(0.0); \ - _R(1,0) = REAL(0.0); \ - _R(1,1) = REAL(1.0); \ - _R(1,2) = REAL(0.0); \ - _R(1,3) = REAL(0.0); \ - _R(2,0) = REAL(0.0); \ - _R(2,1) = REAL(0.0); \ - _R(2,2) = REAL(1.0); \ - _R(2,3) = REAL(0.0); - - -void dRSetIdentity (dMatrix3 R) -{ - dAASSERT (R); - SET_3x3_IDENTITY; -} - - -void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az, - dReal angle) -{ - dAASSERT (R); - dQuaternion q; - dQFromAxisAndAngle (q,ax,ay,az,angle); - dQtoR (q,R); -} - - -void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi) -{ - dReal sphi,cphi,stheta,ctheta,spsi,cpsi; - dAASSERT (R); - sphi = dSin(phi); - cphi = dCos(phi); - stheta = dSin(theta); - ctheta = dCos(theta); - spsi = dSin(psi); - cpsi = dCos(psi); - _R(0,0) = cpsi*ctheta; - _R(0,1) = spsi*ctheta; - _R(0,2) =-stheta; - _R(1,0) = cpsi*stheta*sphi - spsi*cphi; - _R(1,1) = spsi*stheta*sphi + cpsi*cphi; - _R(1,2) = ctheta*sphi; - _R(2,0) = cpsi*stheta*cphi + spsi*sphi; - _R(2,1) = spsi*stheta*cphi - cpsi*sphi; - _R(2,2) = ctheta*cphi; -} - - -void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az, - dReal bx, dReal by, dReal bz) -{ - dReal l,k; - dAASSERT (R); - l = dSqrt (ax*ax + ay*ay + az*az); - if (l <= REAL(0.0)) { - dDEBUGMSG ("zero length vector"); - return; - } - l = dRecip(l); - ax *= l; - ay *= l; - az *= l; - k = ax*bx + ay*by + az*bz; - bx -= k*ax; - by -= k*ay; - bz -= k*az; - l = dSqrt (bx*bx + by*by + bz*bz); - if (l <= REAL(0.0)) { - dDEBUGMSG ("zero length vector"); - return; - } - l = dRecip(l); - bx *= l; - by *= l; - bz *= l; - _R(0,0) = ax; - _R(1,0) = ay; - _R(2,0) = az; - _R(0,1) = bx; - _R(1,1) = by; - _R(2,1) = bz; - _R(0,2) = - by*az + ay*bz; - _R(1,2) = - bz*ax + az*bx; - _R(2,2) = - bx*ay + ax*by; -} - - -void dQSetIdentity (dQuaternion q) -{ - dAASSERT (q); - q[0] = 1; - q[1] = 0; - q[2] = 0; - q[3] = 0; -} - - -void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az, - dReal angle) -{ - dAASSERT (q); - dReal l = ax*ax + ay*ay + az*az; - if (l > REAL(0.0)) { - angle *= REAL(0.5); - q[0] = dCos (angle); - l = dSin(angle) * dRecipSqrt(l); - q[1] = ax*l; - q[2] = ay*l; - q[3] = az*l; - } - else { - q[0] = 1; - q[1] = 0; - q[2] = 0; - q[3] = 0; - } -} - - -void dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) -{ - dAASSERT (qa && qb && qc); - qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3]; - qa[1] = qb[0]*qc[1] + qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2]; - qa[2] = qb[0]*qc[2] + qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3]; - qa[3] = qb[0]*qc[3] + qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1]; -} - - -void dQMultiply1 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) -{ - dAASSERT (qa && qb && qc); - qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3]; - qa[1] = qb[0]*qc[1] - qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2]; - qa[2] = qb[0]*qc[2] - qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3]; - qa[3] = qb[0]*qc[3] - qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1]; -} - - -void dQMultiply2 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) -{ - dAASSERT (qa && qb && qc); - qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3]; - qa[1] = -qb[0]*qc[1] + qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2]; - qa[2] = -qb[0]*qc[2] + qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3]; - qa[3] = -qb[0]*qc[3] + qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1]; -} - - -void dQMultiply3 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) -{ - dAASSERT (qa && qb && qc); - qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3]; - qa[1] = -qb[0]*qc[1] - qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2]; - qa[2] = -qb[0]*qc[2] - qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3]; - qa[3] = -qb[0]*qc[3] - qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1]; -} - - -// QtoR(), RtoQ() and WtoDQ() are derived from equations in "An Introduction -// to Physically Based Modeling: Rigid Body Simulation - 1: Unconstrained -// Rigid Body Dynamics" by David Baraff, Robotics Institute, Carnegie Mellon -// University, 1997. - -void dQtoR (const dQuaternion q, dMatrix3 R) -{ - dAASSERT (q && R); - // q = (s,vx,vy,vz) - dReal qq1 = 2*q[1]*q[1]; - dReal qq2 = 2*q[2]*q[2]; - dReal qq3 = 2*q[3]*q[3]; - _R(0,0) = 1 - qq2 - qq3; - _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]); - _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]); - _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]); - _R(1,1) = 1 - qq1 - qq3; - _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]); - _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]); - _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]); - _R(2,2) = 1 - qq1 - qq2; -} - - -void dRtoQ (const dMatrix3 R, dQuaternion q) -{ - dAASSERT (q && R); - dReal tr,s; - tr = _R(0,0) + _R(1,1) + _R(2,2); - if (tr >= 0) { - s = dSqrt (tr + 1); - q[0] = REAL(0.5) * s; - s = REAL(0.5) * dRecip(s); - q[1] = (_R(2,1) - _R(1,2)) * s; - q[2] = (_R(0,2) - _R(2,0)) * s; - q[3] = (_R(1,0) - _R(0,1)) * s; - } - else { - // find the largest diagonal element and jump to the appropriate case - if (_R(1,1) > _R(0,0)) { - if (_R(2,2) > _R(1,1)) goto case_2; - goto case_1; - } - if (_R(2,2) > _R(0,0)) goto case_2; - goto case_0; - - case_0: - s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + 1); - q[1] = REAL(0.5) * s; - s = REAL(0.5) * dRecip(s); - q[2] = (_R(0,1) + _R(1,0)) * s; - q[3] = (_R(2,0) + _R(0,2)) * s; - q[0] = (_R(2,1) - _R(1,2)) * s; - return; - - case_1: - s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + 1); - q[2] = REAL(0.5) * s; - s = REAL(0.5) * dRecip(s); - q[3] = (_R(1,2) + _R(2,1)) * s; - q[1] = (_R(0,1) + _R(1,0)) * s; - q[0] = (_R(0,2) - _R(2,0)) * s; - return; - - case_2: - s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + 1); - q[3] = REAL(0.5) * s; - s = REAL(0.5) * dRecip(s); - q[1] = (_R(2,0) + _R(0,2)) * s; - q[2] = (_R(1,2) + _R(2,1)) * s; - q[0] = (_R(1,0) - _R(0,1)) * s; - return; - } -} - - -void dWtoDQ (const dVector3 w, const dQuaternion q, dVector4 dq) -{ - dAASSERT (w && q && dq); - dq[0] = REAL(0.5)*(- w[0]*q[1] - w[1]*q[2] - w[2]*q[3]); - dq[1] = REAL(0.5)*( w[0]*q[0] + w[1]*q[3] - w[2]*q[2]); - dq[2] = REAL(0.5)*(- w[0]*q[3] + w[1]*q[0] + w[2]*q[1]); - dq[3] = REAL(0.5)*( w[0]*q[2] - w[1]*q[1] + w[2]*q[0]); -} diff --git a/extern/ode/dist/ode/src/scrapbook.cpp b/extern/ode/dist/ode/src/scrapbook.cpp deleted file mode 100644 index ca8c11f1f1b..00000000000 --- a/extern/ode/dist/ode/src/scrapbook.cpp +++ /dev/null @@ -1,270 +0,0 @@ - -/* - -this is code that was once useful but has now been obseleted. - -this file should not be compiled as part of ODE! - -*/ - -//*************************************************************************** -// intersect a line segment with a plane - -extern "C" int dClipLineToBox (const dVector3 p1, const dVector3 p2, - const dVector3 p, const dMatrix3 R, - const dVector3 side) -{ - // compute the start and end of the line (p1 and p2) relative to the box. - // we will do all subsequent computations in this box-relative coordinate - // system. we have to do a translation and rotation for each point. - dVector3 tmp,s,e; - tmp[0] = p1[0] - p[0]; - tmp[1] = p1[1] - p[1]; - tmp[2] = p1[2] - p[2]; - dMULTIPLY1_331 (s,R,tmp); - tmp[0] = p2[0] - p[0]; - tmp[1] = p2[1] - p[1]; - tmp[2] = p2[2] - p[2]; - dMULTIPLY1_331 (e,R,tmp); - - // compute the vector 'v' from the start point to the end point - dVector3 v; - v[0] = e[0] - s[0]; - v[1] = e[1] - s[1]; - v[2] = e[2] - s[2]; - - // a point on the line is defined by the parameter 't'. t=0 corresponds - // to the start of the line, t=1 corresponds to the end of the line. - // we will clip the line to the box by finding the range of t where a - // point on the line is inside the box. the currently known bounds for - // t and tlo..thi. - dReal tlo=0,thi=1; - - // clip in the X/Y/Z direction - for (int i=0; i<3; i++) { - // first adjust s,e for the current t range. this is redundant for the - // first iteration, but never mind. - e[i] = s[i] + thi*v[i]; - s[i] = s[i] + tlo*v[i]; - // compute where t intersects the positive and negative sides. - dReal tp = ( side[i] - s[i])/v[i]; // @@@ handle case where denom=0 - dReal tm = (-side[i] - s[i])/v[i]; - // handle 9 intersection cases - if (s[i] <= -side[i]) { - tlo = tm; - if (e[i] <= -side[i]) return 0; - else if (e[i] >= side[i]) thi = tp; - } - else if (s[i] <= side[i]) { - if (e[i] <= -side[i]) thi = tm; - else if (e[i] >= side[i]) thi = tp; - } - else { - tlo = tp; - if (e[i] <= -side[i]) thi = tm; - else if (e[i] >= side[i]) return 0; - } - } - - //... @@@ AT HERE @@@ - - return 1; -} - - -//*************************************************************************** -// a nice try at C-B collision. unfortunately it doesn't work. the logic -// for testing for line-box intersection is correct, but unfortunately the -// closest-point distance estimates are often too large. as a result contact -// points are placed incorrectly. - - -int dCollideCB (const dxGeom *o1, const dxGeom *o2, int flags, - dContactGeom *contact, int skip) -{ - int i; - - dIASSERT (skip >= (int)sizeof(dContactGeom)); - dIASSERT (o1->_class->num == dCCylinderClass); - dIASSERT (o2->_class->num == dBoxClass); - contact->g1 = const_cast<dxGeom*> (o1); - contact->g2 = const_cast<dxGeom*> (o2); - dxCCylinder *cyl = (dxCCylinder*) CLASSDATA(o1); - dxBox *box = (dxBox*) CLASSDATA(o2); - - // get p1,p2 = cylinder axis endpoints, get radius - dVector3 p1,p2; - dReal clen = cyl->lz * REAL(0.5); - p1[0] = o1->pos[0] + clen * o1->R[2]; - p1[1] = o1->pos[1] + clen * o1->R[6]; - p1[2] = o1->pos[2] + clen * o1->R[10]; - p2[0] = o1->pos[0] - clen * o1->R[2]; - p2[1] = o1->pos[1] - clen * o1->R[6]; - p2[2] = o1->pos[2] - clen * o1->R[10]; - dReal radius = cyl->radius; - - // copy out box center, rotation matrix, and side array - dReal *c = o2->pos; - dReal *R = o2->R; - dReal *side = box->side; - - // compute the start and end of the line (p1 and p2) relative to the box. - // we will do all subsequent computations in this box-relative coordinate - // system. we have to do a translation and rotation for each point. - dVector3 tmp3,s,e; - tmp3[0] = p1[0] - c[0]; - tmp3[1] = p1[1] - c[1]; - tmp3[2] = p1[2] - c[2]; - dMULTIPLY1_331 (s,R,tmp3); - tmp3[0] = p2[0] - c[0]; - tmp3[1] = p2[1] - c[1]; - tmp3[2] = p2[2] - c[2]; - dMULTIPLY1_331 (e,R,tmp3); - - // compute the vector 'v' from the start point to the end point - dVector3 v; - v[0] = e[0] - s[0]; - v[1] = e[1] - s[1]; - v[2] = e[2] - s[2]; - - // compute the half-sides of the box - dReal S0 = side[0] * REAL(0.5); - dReal S1 = side[1] * REAL(0.5); - dReal S2 = side[2] * REAL(0.5); - - // compute the size of the bounding box around the line segment - dReal B0 = dFabs (v[0]); - dReal B1 = dFabs (v[1]); - dReal B2 = dFabs (v[2]); - - // for all 6 separation axes, measure the penetration depth. if any depth is - // less than 0 then the objects don't penetrate at all so we can just - // return 0. find the axis with the smallest depth, and record its normal. - - // note: normalR is set to point to a column of R if that is the smallest - // depth normal so far. otherwise normalR is 0 and normalC is set to a - // vector relative to the box. invert_normal is 1 if the sign of the normal - // should be flipped. - - dReal depth,trial_depth,tmp,length; - const dReal *normalR=0; - dVector3 normalC; - int invert_normal = 0; - int code = 0; // 0=no contact, 1-3=face contact, 4-6=edge contact - - depth = dInfinity; - - // look at face-normal axes - -#undef TEST -#define TEST(center,depth_expr,norm,contact_code) \ - tmp = (center); \ - trial_depth = radius + REAL(0.5) * ((depth_expr) - dFabs(tmp)); \ - if (trial_depth < 0) return 0; \ - if (trial_depth < depth) { \ - depth = trial_depth; \ - normalR = (norm); \ - invert_normal = (tmp < 0); \ - code = contact_code; \ - } - - TEST (s[0]+e[0], side[0] + B0, R+0, 1); - TEST (s[1]+e[1], side[1] + B1, R+1, 2); - TEST (s[2]+e[2], side[2] + B2, R+2, 3); - - // look at v x box-edge axes - -#undef TEST -#define TEST(box_radius,line_offset,nx,ny,nz,contact_code) \ - tmp = (line_offset); \ - trial_depth = (box_radius) - dFabs(tmp); \ - length = dSqrt ((nx)*(nx) + (ny)*(ny) + (nz)*(nz)); \ - if (length > 0) { \ - length = dRecip(length); \ - trial_depth = trial_depth * length + radius; \ - if (trial_depth < 0) return 0; \ - if (trial_depth < depth) { \ - depth = trial_depth; \ - normalR = 0; \ - normalC[0] = (nx)*length; \ - normalC[1] = (ny)*length; \ - normalC[2] = (nz)*length; \ - invert_normal = (tmp < 0); \ - code = contact_code; \ - } \ - } - - TEST (B2*S1+B1*S2,v[1]*s[2]-v[2]*s[1], 0,-v[2],v[1], 4); - TEST (B2*S0+B0*S2,v[2]*s[0]-v[0]*s[2], v[2],0,-v[0], 5); - TEST (B1*S0+B0*S1,v[0]*s[1]-v[1]*s[0], -v[1],v[0],0, 6); - -#undef TEST - - // if we get to this point, the box and ccylinder interpenetrate. - // compute the normal in global coordinates. - dReal *normal = contact[0].normal; - if (normalR) { - normal[0] = normalR[0]; - normal[1] = normalR[4]; - normal[2] = normalR[8]; - } - else { - dMULTIPLY0_331 (normal,R,normalC); - } - if (invert_normal) { - normal[0] = -normal[0]; - normal[1] = -normal[1]; - normal[2] = -normal[2]; - } - - // set the depth - contact[0].depth = depth; - - if (code == 0) { - return 0; // should never get here - } - else if (code >= 4) { - // handle edge contacts - // find an endpoint q1 on the intersecting edge of the box - dVector3 q1; - dReal sign[3]; - for (i=0; i<3; i++) q1[i] = c[i]; - sign[0] = (dDOT14(normal,R+0) > 0) ? REAL(1.0) : REAL(-1.0); - for (i=0; i<3; i++) q1[i] += sign[0] * S0 * R[i*4]; - sign[1] = (dDOT14(normal,R+1) > 0) ? REAL(1.0) : REAL(-1.0); - for (i=0; i<3; i++) q1[i] += sign[1] * S1 * R[i*4+1]; - sign[2] = (dDOT14(normal,R+2) > 0) ? REAL(1.0) : REAL(-1.0); - for (i=0; i<3; i++) q1[i] += sign[2] * S2 * R[i*4+2]; - - // find the other endpoint q2 of the intersecting edge - dVector3 q2; - for (i=0; i<3; i++) - q2[i] = q1[i] - R[code-4 + i*4] * (sign[code-4] * side[code-4]); - - // determine the closest point between the box edge and the line segment - dVector3 cp1,cp2; - dClosestLineSegmentPoints (q1,q2, p1,p2, cp1,cp2); - for (i=0; i<3; i++) contact[0].pos[i] = cp1[i] - REAL(0.5)*normal[i]*depth; - return 1; - } - else { - // handle face contacts. - // @@@ temporary: make deepest vertex on the line the contact point. - // @@@ this kind of works, but we sometimes need two contact points for - // @@@ stability. - - // compute 'v' in global coordinates - dVector3 gv; - for (i=0; i<3; i++) gv[i] = p2[i] - p1[i]; - - if (dDOT (normal,gv) > 0) { - for (i=0; i<3; i++) - contact[0].pos[i] = p1[i] + (depth*REAL(0.5)-radius)*normal[i]; - } - else { - for (i=0; i<3; i++) - contact[0].pos[i] = p2[i] + (depth*REAL(0.5)-radius)*normal[i]; - } - return 1; - } -} diff --git a/extern/ode/dist/ode/src/space.cpp b/extern/ode/dist/ode/src/space.cpp deleted file mode 100644 index 0c656573918..00000000000 --- a/extern/ode/dist/ode/src/space.cpp +++ /dev/null @@ -1,621 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -/* - -simple space ------------- - -reports all n^2 object intersections - - -multi-resolution hash table ---------------------------- - -the current implementation rebuilds a new hash table each time collide() -is called. we don't keep any state between calls. this is wasteful if there -are unmoving objects in the space. - - -TODO ----- - -less memory wasting may to prevent multiple collision callbacks for the -same pair? - -better virtual address function. - -the collision search can perhaps be optimized - as we search chains we can -come across other candidate intersections at other levels, perhaps we should -do the intersection check straight away? --> save on list searching time only, -which is not too significant. - -*/ - -//**************************************************************************** - -#include <ode/common.h> -#include <ode/space.h> -#include <ode/geom.h> -#include <ode/error.h> -#include <ode/memory.h> -#include "objects.h" -#include "geom_internal.h" - -//**************************************************************************** -// space base class - -struct dxSpace : public dBase { - int type; // don't want to use RTTI - virtual void destroy()=0; - virtual void add (dGeomID)=0; - virtual void remove (dGeomID)=0; - virtual void collide (void *data, dNearCallback *callback)=0; - virtual int query (dGeomID)=0; -}; - -#define TYPE_SIMPLE 0xbad -#define TYPE_HASH 0xbabe - -//**************************************************************************** -// stuff common to all spaces - -#define ALLOCA(x) dALLOCA16(x) - - -// collide two AABBs together. for the hash table space, this is called if -// the two AABBs inhabit the same hash table cells. this only calls the -// callback function if the boxes actually intersect. if a geom has an -// AABB test function, that is called to provide a further refinement of -// the intersection. - -static inline void collideAABBs (dReal bounds1[6], dReal bounds2[6], - dxGeom *g1, dxGeom *g2, - void *data, dNearCallback *callback) -{ - // no contacts if both geoms on the same body, and the body is not 0 - if (g1->body == g2->body && g1->body) return; - - if (bounds1[0] > bounds2[1] || - bounds1[1] < bounds2[0] || - bounds1[2] > bounds2[3] || - bounds1[3] < bounds2[2] || - bounds1[4] > bounds2[5] || - bounds1[5] < bounds2[4]) return; - if (g1->_class->aabb_test) { - if (g1->_class->aabb_test (g1,g2,bounds2) == 0) return; - } - if (g2->_class->aabb_test) { - if (g2->_class->aabb_test (g2,g1,bounds1) == 0) return; - } - callback (data,g1,g2); -} - -//**************************************************************************** -// simple space - reports all n^2 object intersections - -struct dxSimpleSpace : public dxSpace { - dGeomID first; - void destroy(); - void add (dGeomID); - void remove (dGeomID); - void collide (void *data, dNearCallback *callback); - int query (dGeomID); -}; - - -dSpaceID dSimpleSpaceCreate() -{ - dxSimpleSpace *w = new dxSimpleSpace; - w->type = TYPE_SIMPLE; - w->first = 0; - return w; -} - - -void dxSimpleSpace::destroy() -{ - // destroying each geom will call remove(). this will be efficient if - // we destroy geoms in list order. - dAASSERT (this); - dGeomID g,n; - g = first; - while (g) { - n = g->space.next; - dGeomDestroy (g); - g = n; - } - delete this; -} - - -void dxSimpleSpace::add (dGeomID obj) -{ - dAASSERT (this && obj); - dUASSERT (obj->spaceid == 0 && obj->space.next == 0, - "object is already in a space"); - obj->space.next = first; - first = obj; - obj->spaceid = this; -} - - -void dxSimpleSpace::remove (dGeomID geom_to_remove) -{ - dAASSERT (this && geom_to_remove); - dUASSERT (geom_to_remove->spaceid,"object is not in a space"); - dGeomID last=0,g=first; - while (g) { - if (g==geom_to_remove) { - if (last) last->space.next = g->space.next; - else first = g->space.next; - geom_to_remove->space.next = 0; - geom_to_remove->spaceid = 0; - return; - } - last = g; - g = g->space.next; - } -} - - -void dxSimpleSpace::collide (void *data, dNearCallback *callback) -{ - dAASSERT (this && callback); - dxGeom *g1,*g2; - int i,j,n; - - // count the number of objects - n=0; - for (g1=first; g1; g1=g1->space.next) n++; - - // allocate and fill bounds array - dReal *bounds = (dReal*) ALLOCA (6 * n * sizeof(dReal)); - i=0; - for (g1=first; g1; g1=g1->space.next) { - g1->_class->aabb (g1,bounds + i); - g1->space_aabb = bounds + i; - i += 6; - } - - // intersect all bounding boxes - i=0; - for (g1=first; g1; g1=g1->space.next) { - j=i+6; - for (g2=g1->space.next; g2; g2=g2->space.next) { - collideAABBs (bounds+i,bounds+j,g1,g2,data,callback); - j += 6; - } - i += 6; - } - - // reset the aabb fields of the geoms back to 0 - for (g1=first; g1; g1=g1->space.next) g1->space_aabb = 0; -} - - -// @@@ NOT FLEXIBLE ENOUGH -// -//int dSpaceCollide (dSpaceID space, dContactGeom **contact_array) -//{ -// int n = 0; -// dContactGeom *base = (dContact*) dStackAlloc (sizeof(dContact)); -// dContactGeom *c = base; -// for (dxGeom *g1=space->first; g1; g1=g1->space.next) { -// for (dxGeom *g2=g1->space.next; g2; g2=g2->space.next) { -// // generate at most 1 contact for this pair -// c->o1 = g1; -// c->o2 = g2; -// if (dCollide (0,c)) { -// c = (dContactGeom*) dStackAlloc (sizeof(dContactGeom)); -// n++; -// } -// } -// } -// *contact_array = base; -// return n; -//} - - -int dxSimpleSpace::query (dGeomID obj) -{ - dAASSERT (this && obj); - if (obj->spaceid != this) return 0; - dGeomID compare = first; - while (compare) { - if (compare == obj) return 1; - compare = compare->space.next; - } - dDebug (0,"object is not in the space it thinks it is in"); - return 0; -} - -//**************************************************************************** -// hash table space - -// kind of silly, but oh well... -#define MAXINT ((int)((((unsigned int)(-1)) << 1) >> 1)) - - -// prime[i] is the largest prime smaller than 2^i -#define NUM_PRIMES 31 -static long int prime[NUM_PRIMES] = {1L,2L,3L,7L,13L,31L,61L,127L,251L,509L, - 1021L,2039L,4093L,8191L,16381L,32749L,65521L,131071L,262139L, - 524287L,1048573L,2097143L,4194301L,8388593L,16777213L,33554393L, - 67108859L,134217689L,268435399L,536870909L,1073741789L}; - - -// currently the space 'container' is just a list of the geoms in the space. - -struct dxHashSpace : public dxSpace { - dxGeom *first; - int global_minlevel; // smallest hash table level to put AABBs in - int global_maxlevel; // objects that need a level larger than this will be - // put in a "big objects" list instead of a hash table - void destroy(); - void add (dGeomID); - void remove (dGeomID); - void collide (void *data, dNearCallback *callback); - int query (dGeomID); -}; - - -// an axis aligned bounding box -struct dxAABB { - dxAABB *next; // next in the list of all AABBs - dReal bounds[6]; // minx, maxx, miny, maxy, minz, maxz - int level; // the level this is stored in (cell size = 2^level) - int dbounds[6]; // AABB bounds, discretized to cell size - dxGeom *geom; // corresponding geometry object - int index; // index of this AABB, starting from 0 -}; - - -// a hash table node that represents an AABB that intersects a particular cell -// at a particular level -struct Node { - Node *next; // next node in hash table collision list, 0 if none - int x,y,z; // cell position in space, discretized to cell size - dxAABB *aabb; // axis aligned bounding box that intersects this cell -}; - - -// return the `level' of an AABB. the AABB will be put into cells at this -// level - the cell size will be 2^level. the level is chosen to be the -// smallest value such that the AABB occupies no more than 8 cells, regardless -// of its placement. this means that: -// size/2 < q <= size -// where q is the maximum AABB dimension. - -static int findLevel (dReal bounds[6]) -{ - // compute q - dReal q,q2; - q = bounds[1] - bounds[0]; // x bounds - q2 = bounds[3] - bounds[2]; // y bounds - if (q2 > q) q = q2; - q2 = bounds[5] - bounds[4]; // z bounds - if (q2 > q) q = q2; - - if (q == dInfinity) return MAXINT; - - // find level such that 0.5 * 2^level < q <= 2^level - int level; - frexp (q,&level); // q = (0.5 .. 1.0) * 2^level (definition of frexp) - return level; -} - - -// find a virtual memory address for a cell at the given level and x,y,z -// position. -// @@@ currently this is not very sophisticated, e.g. the scaling -// factors could be better designed to avoid collisions, and they should -// probably depend on the hash table physical size. - -static unsigned long getVirtualAddress (int level, int x, int y, int z) -{ - return level*1000 + x*100 + y*10 + z; -} - -//**************************************************************************** -// hash space public functions - -dSpaceID dHashSpaceCreate() -{ - dxHashSpace *w = new dxHashSpace; - w->type = TYPE_HASH; - w->first = 0; - w->global_minlevel = -3; - w->global_maxlevel = 10; - return w; -} - - -void dxHashSpace::destroy() -{ - // destroying each geom will call remove(). this will be efficient if - // we destroy geoms in list order. - dAASSERT (this); - dGeomID g,n; - g = first; - while (g) { - n = g->space.next; - dGeomDestroy (g); - g = n; - } - delete this; -} - - -void dHashSpaceSetLevels (dxSpace *space, int minlevel, int maxlevel) -{ - dUASSERT (minlevel <= maxlevel,"must have minlevel <= maxlevel"); - dUASSERT (space->type == TYPE_HASH,"must be a hash space"); - dxHashSpace *hspace = (dxHashSpace*) space; - hspace->global_minlevel = minlevel; - hspace->global_maxlevel = maxlevel; -} - - -void dxHashSpace::add (dGeomID obj) -{ - dAASSERT (this && obj); - dUASSERT (obj->spaceid == 0 && obj->space.next == 0, - "object is already in a space"); - obj->space.next = first; - first = obj; - obj->spaceid = this; -} - - -void dxHashSpace::remove (dGeomID geom_to_remove) -{ - dAASSERT (this && geom_to_remove); - dUASSERT (geom_to_remove->spaceid,"object is not in a space"); - dGeomID last=0,g=first; - while (g) { - if (g==geom_to_remove) { - if (last) last->space.next = g->space.next; - else first = g->space.next; - geom_to_remove->space.next = 0; - geom_to_remove->spaceid = 0; - return; - } - last = g; - g = g->space.next; - } -} - - -void dxHashSpace::collide (void *data, dNearCallback *callback) -{ - dAASSERT(this && callback); - dxGeom *geom; - dxAABB *aabb; - int i,maxlevel; - - // create a list of axis aligned bounding boxes for all geoms. count the - // number of AABBs as we go. set the level for all AABBs. put AABBs larger - // than the space's global_maxlevel in the big_boxes list, check everything - // else against that list at the end. for AABBs that are not too big, - // record the maximum level that we need. - - int n = 0; // number of AABBs in main list - int ntotal = 0; // total number of AABBs - dxAABB *first_aabb = 0; // list of AABBs in hash table - dxAABB *big_boxes = 0; // list of AABBs too big for hash table - maxlevel = global_minlevel - 1; - for (geom = first; geom; geom=geom->space.next) { - ntotal++; - dxAABB *aabb = (dxAABB*) ALLOCA (sizeof(dxAABB)); - geom->_class->aabb (geom,aabb->bounds); - geom->space_aabb = aabb->bounds; - aabb->geom = geom; - // compute level, but prevent cells from getting too small - int level = findLevel (aabb->bounds); - if (level < global_minlevel) level = global_minlevel; - if (level <= global_maxlevel) { - // aabb goes in main list - aabb->next = first_aabb; - first_aabb = aabb; - aabb->level = level; - if (level > maxlevel) maxlevel = level; - // cellsize = 2^level - dReal cellsize = (dReal) ldexp (1.0,level); - // discretize AABB position to cell size - for (i=0; i < 6; i++) aabb->dbounds[i] = (int) - floor (aabb->bounds[i]/cellsize); - // set AABB index - aabb->index = n; - n++; - } - else { - // aabb is too big, put it in the big_boxes list. we don't care about - // setting level, dbounds, index, or the maxlevel - aabb->next = big_boxes; - big_boxes = aabb; - } - } - - // 0 or 1 boxes can't collide with anything - if (ntotal < 2) return; - - // for `n' objects, an n*n array of bits is used to record if those objects - // have been intersection-tested against each other yet. this array can - // grow large with high n, but oh well... - int tested_rowsize = (n+7) >> 3; // number of bytes needed for n bits - unsigned char *tested = (unsigned char *) alloca (n * tested_rowsize); - memset (tested,0,n * tested_rowsize); - - // create a hash table to store all AABBs. each AABB may take up to 8 cells. - // we use chaining to resolve collisions, but we use a relatively large table - // to reduce the chance of collisions. - - // compute hash table size sz to be a prime > 8*n - for (i=0; i<NUM_PRIMES; i++) { - if (prime[i] >= (8*n)) break; - } - if (i >= NUM_PRIMES) i = NUM_PRIMES-1; // probably pointless - int sz = prime[i]; - - // allocate and initialize hash table node pointers - Node **table = (Node **) ALLOCA (sizeof(Node*) * sz); - for (i=0; i<sz; i++) table[i] = 0; - - // add each AABB to the hash table (may need to add it to up to 8 cells) - for (aabb=first_aabb; aabb; aabb=aabb->next) { - int *dbounds = aabb->dbounds; - for (int xi = dbounds[0]; xi <= dbounds[1]; xi++) { - for (int yi = dbounds[2]; yi <= dbounds[3]; yi++) { - for (int zi = dbounds[4]; zi <= dbounds[5]; zi++) { - // get the hash index - unsigned long hi = getVirtualAddress (aabb->level,xi,yi,zi) % sz; - // add a new node to the hash table - Node *node = (Node*) alloca (sizeof (Node)); - node->x = xi; - node->y = yi; - node->z = zi; - node->aabb = aabb; - node->next = table[hi]; - table[hi] = node; - } - } - } - } - - // now that all AABBs are loaded into the hash table, we do the actual - // collision detection. for all AABBs, check for other AABBs in the - // same cells for collisions, and then check for other AABBs in all - // intersecting higher level cells. - - int db[6]; // discrete bounds at current level - for (aabb=first_aabb; aabb; aabb=aabb->next) { - // we are searching for collisions with aabb - for (i=0; i<6; i++) db[i] = aabb->dbounds[i]; - for (int level = aabb->level; level <= maxlevel; level++) { - for (int xi = db[0]; xi <= db[1]; xi++) { - for (int yi = db[2]; yi <= db[3]; yi++) { - for (int zi = db[4]; zi <= db[5]; zi++) { - // get the hash index - unsigned long hi = getVirtualAddress (level,xi,yi,zi) % sz; - // search all nodes at this index - Node *node; - for (node = table[hi]; node; node=node->next) { - // node points to an AABB that may intersect aabb - if (node->aabb == aabb) continue; - if (node->aabb->level == level && - node->x == xi && node->y == yi && node->z == zi) { - // see if aabb and node->aabb have already been tested - // against each other - unsigned char mask; - if (aabb->index <= node->aabb->index) { - i = (aabb->index * tested_rowsize)+(node->aabb->index >> 3); - mask = 1 << (node->aabb->index & 7); - } - else { - i = (node->aabb->index * tested_rowsize)+(aabb->index >> 3); - mask = 1 << (aabb->index & 7); - } - dIASSERT (i >= 0 && i < (tested_rowsize*n)); - if ((tested[i] & mask)==0) { - collideAABBs (aabb->bounds,node->aabb->bounds, - aabb->geom,node->aabb->geom, - data,callback); - } - tested[i] |= mask; - } - } - } - } - } - // get the discrete bounds for the next level up - for (i=0; i<6; i++) db[i] >>= 1; - } - } - - // every AABB in the normal list must now be intersected against every - // AABB in the big_boxes list. so let's hope there are not too many objects - // in the big_boxes list. - for (aabb=first_aabb; aabb; aabb=aabb->next) { - for (dxAABB *aabb2=big_boxes; aabb2; aabb2=aabb2->next) { - collideAABBs (aabb->bounds,aabb2->bounds,aabb->geom,aabb2->geom, - data,callback); - } - } - - // intersected all AABBs in the big_boxes list together - for (aabb=big_boxes; aabb; aabb=aabb->next) { - for (dxAABB *aabb2=aabb->next; aabb2; aabb2=aabb2->next) { - collideAABBs (aabb->bounds,aabb2->bounds,aabb->geom,aabb2->geom, - data,callback); - } - } - - // reset the aabb fields of the geoms back to 0 - for (geom=first; geom; geom=geom->space.next) geom->space_aabb = 0; -} - - -int dxHashSpace::query (dGeomID obj) -{ - dAASSERT (this && obj); - if (obj->spaceid != this) return 0; - dGeomID compare = first; - while (compare) { - if (compare == obj) return 1; - compare = compare->space.next; - } - dDebug (0,"object is not in the space it thinks it is in"); - return 0; -} - -//**************************************************************************** -// space functions - -void dSpaceDestroy (dxSpace * space) -{ - space->destroy(); -} - - -void dSpaceAdd (dxSpace * space, dxGeom *g) -{ - space->add (g); -} - - -void dSpaceRemove (dxSpace * space, dxGeom *g) -{ - space->remove (g); -} - - -int dSpaceQuery (dxSpace * space, dxGeom *g) -{ - return space->query (g); -} - - -void dSpaceCollide (dxSpace * space, void *data, dNearCallback *callback) -{ - space->collide (data,callback); -} diff --git a/extern/ode/dist/ode/src/stack.cpp b/extern/ode/dist/ode/src/stack.cpp deleted file mode 100644 index e062f92b54f..00000000000 --- a/extern/ode/dist/ode/src/stack.cpp +++ /dev/null @@ -1,114 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -@@@ this file should not be compiled any more @@@ - -#include <string.h> -#include <errno.h> -#include "stack.h" -#include "ode/error.h" -#include "ode/config.h" - -//**************************************************************************** -// unix version that uses mmap(). some systems have anonymous mmaps and some -// need to mmap /dev/zero. - -#ifndef WIN32 - -#include <unistd.h> -#include <sys/mman.h> -#include <sys/types.h> -#include <sys/stat.h> -#include <fcntl.h> - - -void dStack::init (int max_size) -{ - if (sizeof(long int) != sizeof(char*)) dDebug (0,"internal"); - if (max_size <= 0) dDebug (0,"Stack::init() given size <= 0"); - -#ifndef MMAP_ANONYMOUS - static int dev_zero_fd = -1; // cached file descriptor for /dev/zero - if (dev_zero_fd < 0) dev_zero_fd = open ("/dev/zero", O_RDWR); - if (dev_zero_fd < 0) dError (0,"can't open /dev/zero (%s)",strerror(errno)); - base = (char*) mmap (0,max_size, PROT_READ | PROT_WRITE, MAP_PRIVATE, - dev_zero_fd,0); -#else - base = (char*) mmap (0,max_size, PROT_READ | PROT_WRITE, - MAP_PRIVATE | MAP_ANON,0,0); -#endif - - if (int(base) == -1) dError (0,"Stack::init(), mmap() failed, " - "max_size=%d (%s)",max_size,strerror(errno)); - size = max_size; - pointer = base; - frame = 0; -} - - -void dStack::destroy() -{ - munmap (base,size); - base = 0; - size = 0; - pointer = 0; - frame = 0; -} - -#endif - -//**************************************************************************** - -#ifdef WIN32 - -#include "windows.h" - - -void dStack::init (int max_size) -{ - if (sizeof(LPVOID) != sizeof(char*)) dDebug (0,"internal"); - if (max_size <= 0) dDebug (0,"Stack::init() given size <= 0"); - base = (char*) VirtualAlloc (NULL,max_size,MEM_RESERVE,PAGE_READWRITE); - if (base == 0) dError (0,"Stack::init(), VirtualAlloc() failed, " - "max_size=%d",max_size); - size = max_size; - pointer = base; - frame = 0; - committed = 0; - - // get page size - SYSTEM_INFO info; - GetSystemInfo (&info); - pagesize = info.dwPageSize; -} - - -void dStack::destroy() -{ - VirtualFree (base,0,MEM_RELEASE); - base = 0; - size = 0; - pointer = 0; - frame = 0; -} - -#endif diff --git a/extern/ode/dist/ode/src/stack.h b/extern/ode/dist/ode/src/stack.h deleted file mode 100644 index 3b98202b042..00000000000 --- a/extern/ode/dist/ode/src/stack.h +++ /dev/null @@ -1,139 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -/* this comes from the `reuse' library. copy any changes back to the source. - -these stack allocation functions are a replacement for alloca(), except that -they allocate memory from a separate pool. - -advantages over alloca(): - - consecutive allocations are guaranteed to be contiguous with increasing - address. - - functions can allocate stack memory that is returned to the caller, - in other words pushing and popping stack frames is optional. - -disadvantages compared to alloca(): - - less portable - - slightly slower, although still orders of magnitude faster than malloc(). - - longjmp() and exceptions do not deallocate stack memory (but who cares?). - -just like alloca(): - - using too much stack memory does not fail gracefully, it fails with a - segfault. - -*/ - - -#ifndef _ODE_STACK_H_ -#define _ODE_STACK_H_ - - -#ifdef WIN32 -#include "windows.h" -#endif - - -struct dStack { - char *base; // bottom of the stack - int size; // maximum size of the stack - char *pointer; // current top of the stack - char *frame; // linked list of stack frame ptrs -# ifdef WIN32 // stuff for windows: - int pagesize; // - page size - this is ASSUMED to be a power of 2 - int committed; // - bytes committed in allocated region -#endif - - // initialize the stack. `max_size' is the maximum size that the stack can - // reach. on unix and windows a `virtual' memory block of this size is - // mapped into the address space but does not actually consume physical - // memory until it is referenced - so it is safe to set this to a high value. - - void init (int max_size); - - - // destroy the stack. this unmaps any virtual memory that was allocated. - - void destroy(); - - - // allocate `size' bytes from the stack and return a pointer to the allocated - // memory. `size' must be >= 0. the returned pointer will be aligned to the - // size of a long int. - - char * alloc (int size) - { - char *ret = pointer; - pointer += ((size-1) | (sizeof(long int)-1) )+1; -# ifdef WIN32 - // for windows we need to commit pages as they are required - if ((pointer-base) > committed) { - committed = ((pointer-base-1) | (pagesize-1))+1; // round up to pgsize - VirtualAlloc (base,committed,MEM_COMMIT,PAGE_READWRITE); - } -# endif - return ret; - } - - - // return the address that will be returned by the next call to alloc() - - char *nextAlloc() - { - return pointer; - } - - - // push and pop the current size of the stack. pushFrame() saves the current - // frame pointer on the stack, and popFrame() retrieves it. a typical - // stack-using function will bracket alloc() calls with pushFrame() and - // popFrame(). both functions return the current stack pointer - this should - // be the same value for the two bracketing calls. calling popFrame() too - // many times will result in a segfault. - - char * pushFrame() - { - char *newframe = pointer; - char **addr = (char**) alloc (sizeof(char*)); - *addr = frame; - frame = newframe; - return newframe; - - /* OLD CODE - *((char**)pointer) = frame; - frame = pointer; - char *ret = pointer; - pointer += sizeof(char*); - return ret; - */ - } - - char * popFrame() - { - pointer = frame; - frame = *((char**)pointer); - return pointer; - } -}; - - -#endif - diff --git a/extern/ode/dist/ode/src/step.cpp b/extern/ode/dist/ode/src/step.cpp deleted file mode 100644 index 16f77112ad2..00000000000 --- a/extern/ode/dist/ode/src/step.cpp +++ /dev/null @@ -1,1085 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#include "objects.h" -#include "joint.h" -#include <ode/config.h> -#include <ode/odemath.h> -#include <ode/rotation.h> -#include <ode/timer.h> -#include <ode/error.h> -#include <ode/matrix.h> -#include "lcp.h" - -//**************************************************************************** -// misc defines - -#define FAST_FACTOR -//#define TIMING - -#define ALLOCA dALLOCA16 - -//**************************************************************************** -// debugging - comparison of various vectors and matrices produced by the -// slow and fast versions of the stepper. - -//#define COMPARE_METHODS - -#ifdef COMPARE_METHODS -#include "testing.h" -dMatrixComparison comparator; -#endif - -//**************************************************************************** -// special matrix multipliers - -// this assumes the 4th and 8th rows of B and C are zero. - -static void Multiply2_p8r (dReal *A, dReal *B, dReal *C, - int p, int r, int Askip) -{ - int i,j; - dReal sum,*bb,*cc; - dIASSERT (p>0 && r>0 && A && B && C); - bb = B; - for (i=p; i; i--) { - cc = C; - for (j=r; j; j--) { - sum = bb[0]*cc[0]; - sum += bb[1]*cc[1]; - sum += bb[2]*cc[2]; - sum += bb[4]*cc[4]; - sum += bb[5]*cc[5]; - sum += bb[6]*cc[6]; - *(A++) = sum; - cc += 8; - } - A += Askip - r; - bb += 8; - } -} - - -// this assumes the 4th and 8th rows of B and C are zero. - -static void MultiplyAdd2_p8r (dReal *A, dReal *B, dReal *C, - int p, int r, int Askip) -{ - int i,j; - dReal sum,*bb,*cc; - dIASSERT (p>0 && r>0 && A && B && C); - bb = B; - for (i=p; i; i--) { - cc = C; - for (j=r; j; j--) { - sum = bb[0]*cc[0]; - sum += bb[1]*cc[1]; - sum += bb[2]*cc[2]; - sum += bb[4]*cc[4]; - sum += bb[5]*cc[5]; - sum += bb[6]*cc[6]; - *(A++) += sum; - cc += 8; - } - A += Askip - r; - bb += 8; - } -} - - -// this assumes the 4th and 8th rows of B are zero. - -static void Multiply0_p81 (dReal *A, dReal *B, dReal *C, int p) -{ - int i; - dIASSERT (p>0 && A && B && C); - dReal sum; - for (i=p; i; i--) { - sum = B[0]*C[0]; - sum += B[1]*C[1]; - sum += B[2]*C[2]; - sum += B[4]*C[4]; - sum += B[5]*C[5]; - sum += B[6]*C[6]; - *(A++) = sum; - B += 8; - } -} - - -// this assumes the 4th and 8th rows of B are zero. - -static void MultiplyAdd0_p81 (dReal *A, dReal *B, dReal *C, int p) -{ - int i; - dIASSERT (p>0 && A && B && C); - dReal sum; - for (i=p; i; i--) { - sum = B[0]*C[0]; - sum += B[1]*C[1]; - sum += B[2]*C[2]; - sum += B[4]*C[4]; - sum += B[5]*C[5]; - sum += B[6]*C[6]; - *(A++) += sum; - B += 8; - } -} - - -// this assumes the 4th and 8th rows of B are zero. - -static void MultiplyAdd1_8q1 (dReal *A, dReal *B, dReal *C, int q) -{ - int k; - dReal sum; - dIASSERT (q>0 && A && B && C); - sum = 0; - for (k=0; k<q; k++) sum += B[k*8] * C[k]; - A[0] += sum; - sum = 0; - for (k=0; k<q; k++) sum += B[1+k*8] * C[k]; - A[1] += sum; - sum = 0; - for (k=0; k<q; k++) sum += B[2+k*8] * C[k]; - A[2] += sum; - sum = 0; - for (k=0; k<q; k++) sum += B[4+k*8] * C[k]; - A[4] += sum; - sum = 0; - for (k=0; k<q; k++) sum += B[5+k*8] * C[k]; - A[5] += sum; - sum = 0; - for (k=0; k<q; k++) sum += B[6+k*8] * C[k]; - A[6] += sum; -} - - -// this assumes the 4th and 8th rows of B are zero. - -static void Multiply1_8q1 (dReal *A, dReal *B, dReal *C, int q) -{ - int k; - dReal sum; - dIASSERT (q>0 && A && B && C); - sum = 0; - for (k=0; k<q; k++) sum += B[k*8] * C[k]; - A[0] = sum; - sum = 0; - for (k=0; k<q; k++) sum += B[1+k*8] * C[k]; - A[1] = sum; - sum = 0; - for (k=0; k<q; k++) sum += B[2+k*8] * C[k]; - A[2] = sum; - sum = 0; - for (k=0; k<q; k++) sum += B[4+k*8] * C[k]; - A[4] = sum; - sum = 0; - for (k=0; k<q; k++) sum += B[5+k*8] * C[k]; - A[5] = sum; - sum = 0; - for (k=0; k<q; k++) sum += B[6+k*8] * C[k]; - A[6] = sum; -} - -//**************************************************************************** -// body rotation - -// return sin(x)/x. this has a singularity at 0 so special handling is needed -// for small arguments. - -static inline dReal sinc (dReal x) -{ - // if |x| < 1e-4 then use a taylor series expansion. this two term expansion - // is actually accurate to one LS bit within this range if double precision - // is being used - so don't worry! - if (dFabs(x) < 1.0e-4) return REAL(1.0) - x*x*REAL(0.166666666666666666667); - else return dSin(x)/x; -} - - -// given a body b, apply its linear and angular rotation over the time -// interval h, thereby adjusting its position and orientation. - -static inline void moveAndRotateBody (dxBody *b, dReal h) -{ - int j; - - // handle linear velocity - for (j=0; j<3; j++) b->pos[j] += h * b->lvel[j]; - - if (b->flags & dxBodyFlagFiniteRotation) { - dVector3 irv; // infitesimal rotation vector - dQuaternion q; // quaternion for finite rotation - - if (b->flags & dxBodyFlagFiniteRotationAxis) { - // split the angular velocity vector into a component along the finite - // rotation axis, and a component orthogonal to it. - dVector3 frv,irv; // finite rotation vector - dReal k = dDOT (b->finite_rot_axis,b->avel); - frv[0] = b->finite_rot_axis[0] * k; - frv[1] = b->finite_rot_axis[1] * k; - frv[2] = b->finite_rot_axis[2] * k; - irv[0] = b->avel[0] - frv[0]; - irv[1] = b->avel[1] - frv[1]; - irv[2] = b->avel[2] - frv[2]; - - // make a rotation quaternion q that corresponds to frv * h. - // compare this with the full-finite-rotation case below. - h *= REAL(0.5); - dReal theta = k * h; - q[0] = dCos(theta); - dReal s = sinc(theta) * h; - q[1] = frv[0] * s; - q[2] = frv[1] * s; - q[3] = frv[2] * s; - } - else { - // make a rotation quaternion q that corresponds to w * h - dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] + - b->avel[2]*b->avel[2]); - h *= REAL(0.5); - dReal theta = wlen * h; - q[0] = dCos(theta); - dReal s = sinc(theta) * h; - q[1] = b->avel[0] * s; - q[2] = b->avel[1] * s; - q[3] = b->avel[2] * s; - } - - // do the finite rotation - dQuaternion q2; - dQMultiply0 (q2,q,b->q); - for (j=0; j<4; j++) b->q[j] = q2[j]; - - // do the infitesimal rotation if required - if (b->flags & dxBodyFlagFiniteRotationAxis) { - dReal dq[4]; - dWtoDQ (irv,b->q,dq); - for (j=0; j<4; j++) b->q[j] += h * dq[j]; - } - } - else { - // the normal way - do an infitesimal rotation - dReal dq[4]; - dWtoDQ (b->avel,b->q,dq); - for (j=0; j<4; j++) b->q[j] += h * dq[j]; - } - - // normalize the quaternion and convert it to a rotation matrix - dNormalize4 (b->q); - dQtoR (b->q,b->R); -} - -//**************************************************************************** -// the slow, but sure way -// note that this does not do any joint feedback! - -// given lists of bodies and joints that form an island, perform a first -// order timestep. -// -// `body' is the body array, `nb' is the size of the array. -// `_joint' is the body array, `nj' is the size of the array. - -void dInternalStepIsland_x1 (dxWorld *world, dxBody * const *body, int nb, - dxJoint * const *_joint, int nj, dReal stepsize) -{ - int i,j,k; - int n6 = 6*nb; - -# ifdef TIMING - dTimerStart("preprocessing"); -# endif - - // number all bodies in the body list - set their tag values - for (i=0; i<nb; i++) body[i]->tag = i; - - // make a local copy of the joint array, because we might want to modify it. - // (the "dxJoint *const*" declaration says we're allowed to modify the joints - // but not the joint array, because the caller might need it unchanged). - dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*)); - memcpy (joint,_joint,nj * sizeof(dxJoint*)); - - // for all bodies, compute the inertia tensor and its inverse in the global - // frame, and compute the rotational force and add it to the torque - // accumulator. - // @@@ check computation of rotational force. - dReal *I = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal)); - dReal *invI = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal)); - dSetZero (I,3*nb*4); - dSetZero (invI,3*nb*4); - for (i=0; i<nb; i++) { - dReal tmp[12]; - // compute inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->R); - dMULTIPLY0_333 (I+i*12,body[i]->R,tmp); - // compute inverse inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R); - dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp); - // compute rotational force - dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel); - dCROSS (body[i]->tacc,-=,body[i]->avel,tmp); - } - - // add the gravity force to all bodies - for (i=0; i<nb; i++) { - if ((body[i]->flags & dxBodyNoGravity)==0) { - body[i]->facc[0] += body[i]->mass.mass * world->gravity[0]; - body[i]->facc[1] += body[i]->mass.mass * world->gravity[1]; - body[i]->facc[2] += body[i]->mass.mass * world->gravity[2]; - } - } - - // get m = total constraint dimension, nub = number of unbounded variables. - // create constraint offset array and number-of-rows array for all joints. - // the constraints are re-ordered as follows: the purely unbounded - // constraints, the mixed unbounded + LCP constraints, and last the purely - // LCP constraints. - // - // joints with m=0 are inactive and are removed from the joints array - // entirely, so that the code that follows does not consider them. - int m = 0; - dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1)); - int *ofs = (int*) ALLOCA (nj*sizeof(int)); - for (i=0, j=0; j<nj; j++) { // i=dest, j=src - joint[j]->vtable->getInfo1 (joint[j],info+i); - dIASSERT (info[i].m >= 0 && info[i].m <= 6 && - info[i].nub >= 0 && info[i].nub <= info[i].m); - if (info[i].m > 0) { - joint[i] = joint[j]; - i++; - } - } - nj = i; - - // the purely unbounded constraints - for (i=0; i<nj; i++) if (info[i].nub == info[i].m) { - ofs[i] = m; - m += info[i].m; - } - int nub = m; - // the mixed unbounded + LCP constraints - for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) { - ofs[i] = m; - m += info[i].m; - } - // the purely LCP constraints - for (i=0; i<nj; i++) if (info[i].nub == 0) { - ofs[i] = m; - m += info[i].m; - } - - // create (6*nb,6*nb) inverse mass matrix `invM', and fill it with mass - // parameters -# ifdef TIMING - dTimerNow ("create mass matrix"); -# endif - int nskip = dPAD (n6); - dReal *invM = (dReal*) ALLOCA (n6*nskip*sizeof(dReal)); - dSetZero (invM,n6*nskip); - for (i=0; i<nb; i++) { - dReal *MM = invM+(i*6)*nskip+(i*6); - MM[0] = body[i]->invMass; - MM[nskip+1] = body[i]->invMass; - MM[2*nskip+2] = body[i]->invMass; - MM += 3*nskip+3; - for (j=0; j<3; j++) for (k=0; k<3; k++) { - MM[j*nskip+k] = invI[i*12+j*4+k]; - } - } - - // assemble some body vectors: fe = external forces, v = velocities - dReal *fe = (dReal*) ALLOCA (n6 * sizeof(dReal)); - dReal *v = (dReal*) ALLOCA (n6 * sizeof(dReal)); - dSetZero (fe,n6); - dSetZero (v,n6); - for (i=0; i<nb; i++) { - for (j=0; j<3; j++) fe[i*6+j] = body[i]->facc[j]; - for (j=0; j<3; j++) fe[i*6+3+j] = body[i]->tacc[j]; - for (j=0; j<3; j++) v[i*6+j] = body[i]->lvel[j]; - for (j=0; j<3; j++) v[i*6+3+j] = body[i]->avel[j]; - } - - // this will be set to the velocity update - dReal *vnew = (dReal*) ALLOCA (n6 * sizeof(dReal)); - dSetZero (vnew,n6); - - // if there are constraints, compute cforce - if (m > 0) { - // create a constraint equation right hand side vector `c', a constraint - // force mixing vector `cfm', and LCP low and high bound vectors, and an - // 'findex' vector. - dReal *c = (dReal*) ALLOCA (m*sizeof(dReal)); - dReal *cfm = (dReal*) ALLOCA (m*sizeof(dReal)); - dReal *lo = (dReal*) ALLOCA (m*sizeof(dReal)); - dReal *hi = (dReal*) ALLOCA (m*sizeof(dReal)); - int *findex = (int*) alloca (m*sizeof(int)); - dSetZero (c,m); - dSetValue (cfm,m,world->global_cfm); - dSetValue (lo,m,-dInfinity); - dSetValue (hi,m, dInfinity); - for (i=0; i<m; i++) findex[i] = -1; - - // create (m,6*nb) jacobian mass matrix `J', and fill it with constraint - // data. also fill the c vector. -# ifdef TIMING - dTimerNow ("create J"); -# endif - dReal *J = (dReal*) ALLOCA (m*nskip*sizeof(dReal)); - dSetZero (J,m*nskip); - dxJoint::Info2 Jinfo; - Jinfo.rowskip = nskip; - Jinfo.fps = dRecip(stepsize); - Jinfo.erp = world->global_erp; - for (i=0; i<nj; i++) { - Jinfo.J1l = J + nskip*ofs[i] + 6*joint[i]->node[0].body->tag; - Jinfo.J1a = Jinfo.J1l + 3; - if (joint[i]->node[1].body) { - Jinfo.J2l = J + nskip*ofs[i] + 6*joint[i]->node[1].body->tag; - Jinfo.J2a = Jinfo.J2l + 3; - } - else { - Jinfo.J2l = 0; - Jinfo.J2a = 0; - } - Jinfo.c = c + ofs[i]; - Jinfo.cfm = cfm + ofs[i]; - Jinfo.lo = lo + ofs[i]; - Jinfo.hi = hi + ofs[i]; - Jinfo.findex = findex + ofs[i]; - joint[i]->vtable->getInfo2 (joint[i],&Jinfo); - // adjust returned findex values for global index numbering - for (j=0; j<info[i].m; j++) { - if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i]; - } - } - - // compute A = J*invM*J' -# ifdef TIMING - dTimerNow ("compute A"); -# endif - dReal *JinvM = (dReal*) ALLOCA (m*nskip*sizeof(dReal)); - dSetZero (JinvM,m*nskip); - dMultiply0 (JinvM,J,invM,m,n6,n6); - int mskip = dPAD(m); - dReal *A = (dReal*) ALLOCA (m*mskip*sizeof(dReal)); - dSetZero (A,m*mskip); - dMultiply2 (A,JinvM,J,m,n6,m); - - // add cfm to the diagonal of A - for (i=0; i<m; i++) A[i*mskip+i] += cfm[i] * Jinfo.fps; - -# ifdef COMPARE_METHODS - comparator.nextMatrix (A,m,m,1,"A"); -# endif - - // compute `rhs', the right hand side of the equation J*a=c -# ifdef TIMING - dTimerNow ("compute rhs"); -# endif - dReal *tmp1 = (dReal*) ALLOCA (n6 * sizeof(dReal)); - dSetZero (tmp1,n6); - dMultiply0 (tmp1,invM,fe,n6,n6,1); - for (i=0; i<n6; i++) tmp1[i] += v[i]/stepsize; - dReal *rhs = (dReal*) ALLOCA (m * sizeof(dReal)); - dSetZero (rhs,m); - dMultiply0 (rhs,J,tmp1,m,n6,1); - for (i=0; i<m; i++) rhs[i] = c[i]/stepsize - rhs[i]; - -# ifdef COMPARE_METHODS - comparator.nextMatrix (c,m,1,0,"c"); - comparator.nextMatrix (rhs,m,1,0,"rhs"); -# endif - - // solve the LCP problem and get lambda. - // this will destroy A but that's okay -# ifdef TIMING - dTimerNow ("solving LCP problem"); -# endif - dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal)); - dReal *residual = (dReal*) ALLOCA (m * sizeof(dReal)); - dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex); - -// OLD WAY - direct factor and solve -// -// // factorize A (L*L'=A) -//# ifdef TIMING -// dTimerNow ("factorize A"); -//# endif -// dReal *L = (dReal*) ALLOCA (m*mskip*sizeof(dReal)); -// memcpy (L,A,m*mskip*sizeof(dReal)); -// if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite"); -// -// // compute lambda -//# ifdef TIMING -// dTimerNow ("compute lambda"); -//# endif -// dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal)); -// memcpy (lambda,rhs,m * sizeof(dReal)); -// dSolveCholesky (L,lambda,m); - -# ifdef COMPARE_METHODS - comparator.nextMatrix (lambda,m,1,0,"lambda"); -# endif - - // compute the velocity update `vnew' -# ifdef TIMING - dTimerNow ("compute velocity update"); -# endif - dMultiply1 (tmp1,J,lambda,n6,m,1); - for (i=0; i<n6; i++) tmp1[i] += fe[i]; - dMultiply0 (vnew,invM,tmp1,n6,n6,1); - for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i]; - - // see if the constraint has worked: compute J*vnew and make sure it equals - // `c' (to within a certain tolerance). -# ifdef TIMING - dTimerNow ("verify constraint equation"); -# endif - dMultiply0 (tmp1,J,vnew,m,n6,1); - dReal err = 0; - for (i=0; i<m; i++) err += dFabs(tmp1[i]-c[i]); - printf ("%.6e\n",err); - } - else { - // no constraints - dMultiply0 (vnew,invM,fe,n6,n6,1); - for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i]; - } - -# ifdef COMPARE_METHODS - comparator.nextMatrix (vnew,n6,1,0,"vnew"); -# endif - - // apply the velocity update to the bodies -# ifdef TIMING - dTimerNow ("update velocity"); -# endif - for (i=0; i<nb; i++) { - for (j=0; j<3; j++) body[i]->lvel[j] = vnew[i*6+j]; - for (j=0; j<3; j++) body[i]->avel[j] = vnew[i*6+3+j]; - } - - // update the position and orientation from the new linear/angular velocity - // (over the given timestep) -# ifdef TIMING - dTimerNow ("update position"); -# endif - for (i=0; i<nb; i++) moveAndRotateBody (body[i],stepsize); - -# ifdef TIMING - dTimerNow ("tidy up"); -# endif - - // zero all force accumulators - for (i=0; i<nb; i++) { - body[i]->facc[0] = 0; - body[i]->facc[1] = 0; - body[i]->facc[2] = 0; - body[i]->facc[3] = 0; - body[i]->tacc[0] = 0; - body[i]->tacc[1] = 0; - body[i]->tacc[2] = 0; - body[i]->tacc[3] = 0; - } - -# ifdef TIMING - dTimerEnd(); - if (m > 0) dTimerReport (stdout,1); -# endif -} - -//**************************************************************************** -// an optimized version of dInternalStepIsland1() - -void dInternalStepIsland_x2 (dxWorld *world, dxBody * const *body, int nb, - dxJoint * const *_joint, int nj, dReal stepsize) -{ - int i,j,k; -# ifdef TIMING - dTimerStart("preprocessing"); -# endif - - dReal stepsize1 = dRecip(stepsize); - - // number all bodies in the body list - set their tag values - for (i=0; i<nb; i++) body[i]->tag = i; - - // make a local copy of the joint array, because we might want to modify it. - // (the "dxJoint *const*" declaration says we're allowed to modify the joints - // but not the joint array, because the caller might need it unchanged). - dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*)); - memcpy (joint,_joint,nj * sizeof(dxJoint*)); - - // for all bodies, compute the inertia tensor and its inverse in the global - // frame, and compute the rotational force and add it to the torque - // accumulator. I and invI are vertically stacked 3x4 matrices, one per body. - // @@@ check computation of rotational force. - dReal *I = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal)); - dReal *invI = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal)); - dSetZero (I,3*nb*4); - dSetZero (invI,3*nb*4); - for (i=0; i<nb; i++) { - dReal tmp[12]; - // compute inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->R); - dMULTIPLY0_333 (I+i*12,body[i]->R,tmp); - // compute inverse inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R); - dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp); - // compute rotational force - dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel); - dCROSS (body[i]->tacc,-=,body[i]->avel,tmp); - } - - // add the gravity force to all bodies - for (i=0; i<nb; i++) { - if ((body[i]->flags & dxBodyNoGravity)==0) { - body[i]->facc[0] += body[i]->mass.mass * world->gravity[0]; - body[i]->facc[1] += body[i]->mass.mass * world->gravity[1]; - body[i]->facc[2] += body[i]->mass.mass * world->gravity[2]; - } - } - - // get m = total constraint dimension, nub = number of unbounded variables. - // create constraint offset array and number-of-rows array for all joints. - // the constraints are re-ordered as follows: the purely unbounded - // constraints, the mixed unbounded + LCP constraints, and last the purely - // LCP constraints. this assists the LCP solver to put all unbounded - // variables at the start for a quick factorization. - // - // joints with m=0 are inactive and are removed from the joints array - // entirely, so that the code that follows does not consider them. - // also number all active joints in the joint list (set their tag values). - // inactive joints receive a tag value of -1. - - int m = 0; - dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1)); - int *ofs = (int*) ALLOCA (nj*sizeof(int)); - for (i=0, j=0; j<nj; j++) { // i=dest, j=src - joint[j]->vtable->getInfo1 (joint[j],info+i); - dIASSERT (info[i].m >= 0 && info[i].m <= 6 && - info[i].nub >= 0 && info[i].nub <= info[i].m); - if (info[i].m > 0) { - joint[i] = joint[j]; - joint[i]->tag = i; - i++; - } - else { - joint[j]->tag = -1; - } - } - nj = i; - - // the purely unbounded constraints - for (i=0; i<nj; i++) if (info[i].nub == info[i].m) { - ofs[i] = m; - m += info[i].m; - } - int nub = m; - // the mixed unbounded + LCP constraints - for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) { - ofs[i] = m; - m += info[i].m; - } - // the purely LCP constraints - for (i=0; i<nj; i++) if (info[i].nub == 0) { - ofs[i] = m; - m += info[i].m; - } - - // this will be set to the force due to the constraints - dReal *cforce = (dReal*) ALLOCA (nb*8 * sizeof(dReal)); - dSetZero (cforce,nb*8); - - // if there are constraints, compute cforce - if (m > 0) { - // create a constraint equation right hand side vector `c', a constraint - // force mixing vector `cfm', and LCP low and high bound vectors, and an - // 'findex' vector. - dReal *c = (dReal*) ALLOCA (m*sizeof(dReal)); - dReal *cfm = (dReal*) ALLOCA (m*sizeof(dReal)); - dReal *lo = (dReal*) ALLOCA (m*sizeof(dReal)); - dReal *hi = (dReal*) ALLOCA (m*sizeof(dReal)); - int *findex = (int*) alloca (m*sizeof(int)); - dSetZero (c,m); - dSetValue (cfm,m,world->global_cfm); - dSetValue (lo,m,-dInfinity); - dSetValue (hi,m, dInfinity); - for (i=0; i<m; i++) findex[i] = -1; - - // get jacobian data from constraints. a (2*m)x8 matrix will be created - // to store the two jacobian blocks from each constraint. it has this - // format: - // - // l l l 0 a a a 0 \ - // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows) - // l l l 0 a a a 0 / - // l l l 0 a a a 0 \ - // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows) - // l l l 0 a a a 0 / - // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row) - // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row) - // etc... - // - // (lll) = linear jacobian data - // (aaa) = angular jacobian data - // -# ifdef TIMING - dTimerNow ("create J"); -# endif - dReal *J = (dReal*) ALLOCA (2*m*8*sizeof(dReal)); - dSetZero (J,2*m*8); - dxJoint::Info2 Jinfo; - Jinfo.rowskip = 8; - Jinfo.fps = stepsize1; - Jinfo.erp = world->global_erp; - for (i=0; i<nj; i++) { - Jinfo.J1l = J + 2*8*ofs[i]; - Jinfo.J1a = Jinfo.J1l + 4; - Jinfo.J2l = Jinfo.J1l + 8*info[i].m; - Jinfo.J2a = Jinfo.J2l + 4; - Jinfo.c = c + ofs[i]; - Jinfo.cfm = cfm + ofs[i]; - Jinfo.lo = lo + ofs[i]; - Jinfo.hi = hi + ofs[i]; - Jinfo.findex = findex + ofs[i]; - joint[i]->vtable->getInfo2 (joint[i],&Jinfo); - // adjust returned findex values for global index numbering - for (j=0; j<info[i].m; j++) { - if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i]; - } - } - - // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same - // format as J so we just go through the constraints in J multiplying by - // the appropriate scalars and matrices. -# ifdef TIMING - dTimerNow ("compute A"); -# endif - dReal *JinvM = (dReal*) ALLOCA (2*m*8*sizeof(dReal)); - dSetZero (JinvM,2*m*8); - for (i=0; i<nj; i++) { - int b = joint[i]->node[0].body->tag; - dReal body_invMass = body[b]->invMass; - dReal *body_invI = invI + b*12; - dReal *Jsrc = J + 2*8*ofs[i]; - dReal *Jdst = JinvM + 2*8*ofs[i]; - for (j=info[i].m-1; j>=0; j--) { - for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass; - dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI); - Jsrc += 8; - Jdst += 8; - } - if (joint[i]->node[1].body) { - b = joint[i]->node[1].body->tag; - body_invMass = body[b]->invMass; - body_invI = invI + b*12; - for (j=info[i].m-1; j>=0; j--) { - for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass; - dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI); - Jsrc += 8; - Jdst += 8; - } - } - } - - // now compute A = JinvM * J'. A's rows and columns are grouped by joint, - // i.e. in the same way as the rows of J. block (i,j) of A is only nonzero - // if joints i and j have at least one body in common. this fact suggests - // the algorithm used to fill A: - // - // for b = all bodies - // n = number of joints attached to body b - // for i = 1..n - // for j = i+1..n - // ii = actual joint number for i - // jj = actual joint number for j - // // (ii,jj) will be set to all pairs of joints around body b - // compute blockwise: A(ii,jj) += JinvM(ii) * J(jj)' - // - // this algorithm catches all pairs of joints that have at least one body - // in common. it does not compute the diagonal blocks of A however - - // another similar algorithm does that. - - int mskip = dPAD(m); - dReal *A = (dReal*) ALLOCA (m*mskip*sizeof(dReal)); - dSetZero (A,m*mskip); - for (i=0; i<nb; i++) { - for (dxJointNode *n1=body[i]->firstjoint; n1; n1=n1->next) { - for (dxJointNode *n2=n1->next; n2; n2=n2->next) { - // get joint numbers and ensure ofs[j1] >= ofs[j2] - int j1 = n1->joint->tag; - int j2 = n2->joint->tag; - if (ofs[j1] < ofs[j2]) { - int tmp = j1; - j1 = j2; - j2 = tmp; - } - - // if either joint was tagged as -1 then it is an inactive (m=0) - // joint that should not be considered - if (j1==-1 || j2==-1) continue; - - // determine if body i is the 1st or 2nd body of joints j1 and j2 - int jb1 = (joint[j1]->node[1].body == body[i]); - int jb2 = (joint[j2]->node[1].body == body[i]); - // jb1/jb2 must be 0 for joints with only one body - dIASSERT(joint[j1]->node[1].body || jb1==0); - dIASSERT(joint[j2]->node[1].body || jb2==0); - - // set block of A - MultiplyAdd2_p8r (A + ofs[j1]*mskip + ofs[j2], - JinvM + 2*8*ofs[j1] + jb1*8*info[j1].m, - J + 2*8*ofs[j2] + jb2*8*info[j2].m, - info[j1].m,info[j2].m, mskip); - } - } - } - // compute diagonal blocks of A - for (i=0; i<nj; i++) { - Multiply2_p8r (A + ofs[i]*(mskip+1), - JinvM + 2*8*ofs[i], - J + 2*8*ofs[i], - info[i].m,info[i].m, mskip); - if (joint[i]->node[1].body) { - MultiplyAdd2_p8r (A + ofs[i]*(mskip+1), - JinvM + 2*8*ofs[i] + 8*info[i].m, - J + 2*8*ofs[i] + 8*info[i].m, - info[i].m,info[i].m, mskip); - } - } - - // add cfm to the diagonal of A - for (i=0; i<m; i++) A[i*mskip+i] += cfm[i] * stepsize1; - -# ifdef COMPARE_METHODS - comparator.nextMatrix (A,m,m,1,"A"); -# endif - - // compute the right hand side `rhs' -# ifdef TIMING - dTimerNow ("compute rhs"); -# endif - dReal *tmp1 = (dReal*) ALLOCA (nb*8 * sizeof(dReal)); - dSetZero (tmp1,nb*8); - // put v/h + invM*fe into tmp1 - for (i=0; i<nb; i++) { - dReal body_invMass = body[i]->invMass; - dReal *body_invI = invI + i*12; - for (j=0; j<3; j++) tmp1[i*8+j] = body[i]->facc[j] * body_invMass + - body[i]->lvel[j] * stepsize1; - dMULTIPLY0_331 (tmp1 + i*8 + 4,body_invI,body[i]->tacc); - for (j=0; j<3; j++) tmp1[i*8+4+j] += body[i]->avel[j] * stepsize1; - } - // put J*tmp1 into rhs - dReal *rhs = (dReal*) ALLOCA (m * sizeof(dReal)); - dSetZero (rhs,m); - for (i=0; i<nj; i++) { - dReal *JJ = J + 2*8*ofs[i]; - Multiply0_p81 (rhs+ofs[i],JJ, - tmp1 + 8*joint[i]->node[0].body->tag, info[i].m); - if (joint[i]->node[1].body) { - MultiplyAdd0_p81 (rhs+ofs[i],JJ + 8*info[i].m, - tmp1 + 8*joint[i]->node[1].body->tag, info[i].m); - } - } - // complete rhs - for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i]; - -# ifdef COMPARE_METHODS - comparator.nextMatrix (c,m,1,0,"c"); - comparator.nextMatrix (rhs,m,1,0,"rhs"); -# endif - - // solve the LCP problem and get lambda. - // this will destroy A but that's okay -# ifdef TIMING - dTimerNow ("solving LCP problem"); -# endif - dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal)); - dReal *residual = (dReal*) ALLOCA (m * sizeof(dReal)); - dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex); - -// OLD WAY - direct factor and solve -// -// // factorize A (L*L'=A) -//# ifdef TIMING -// dTimerNow ("factorize A"); -//# endif -// dReal *L = (dReal*) ALLOCA (m*mskip*sizeof(dReal)); -// memcpy (L,A,m*mskip*sizeof(dReal)); -//# ifdef FAST_FACTOR -// dFastFactorCholesky (L,m); // does not report non positive definiteness -//# else -// if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite"); -//# endif -// -// // compute lambda -//# ifdef TIMING -// dTimerNow ("compute lambda"); -//# endif -// dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal)); -// memcpy (lambda,rhs,m * sizeof(dReal)); -// dSolveCholesky (L,lambda,m); - -# ifdef COMPARE_METHODS - comparator.nextMatrix (lambda,m,1,0,"lambda"); -# endif - - // compute the constraint force `cforce' -# ifdef TIMING - dTimerNow ("compute constraint force"); -# endif - // compute cforce = J'*lambda - for (i=0; i<nj; i++) { - dReal *JJ = J + 2*8*ofs[i]; - dxBody* b1 = joint[i]->node[0].body; - dxBody* b2 = joint[i]->node[1].body; - dJointFeedback *fb = joint[i]->feedback; - - if (fb) { - // the user has requested feedback on the amount of force that this - // joint is applying to the bodies. we use a slightly slower - // computation that splits out the force components and puts them - // in the feedback structure. - dReal data1[8],data2[8]; - Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m); - dReal *cf1 = cforce + 8*b1->tag; - cf1[0] += (fb->f1[0] = data1[0]); - cf1[1] += (fb->f1[1] = data1[1]); - cf1[2] += (fb->f1[2] = data1[2]); - cf1[4] += (fb->t1[0] = data1[4]); - cf1[5] += (fb->t1[1] = data1[5]); - cf1[6] += (fb->t1[2] = data1[6]); - if (b2){ - Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m); - dReal *cf2 = cforce + 8*b2->tag; - cf2[0] += (fb->f2[0] = data2[0]); - cf2[1] += (fb->f2[1] = data2[1]); - cf2[2] += (fb->f2[2] = data2[2]); - cf2[4] += (fb->t2[0] = data2[4]); - cf2[5] += (fb->t2[1] = data2[5]); - cf2[6] += (fb->t2[2] = data2[6]); - } - } - else { - // no feedback is required, let's compute cforce the faster way - MultiplyAdd1_8q1 (cforce + 8*b1->tag,JJ, lambda+ofs[i], info[i].m); - if (b2) { - MultiplyAdd1_8q1 (cforce + 8*b2->tag, - JJ + 8*info[i].m, lambda+ofs[i], info[i].m); - } - } - } - } - - // compute the velocity update -# ifdef TIMING - dTimerNow ("compute velocity update"); -# endif - - // add fe to cforce - for (i=0; i<nb; i++) { - for (j=0; j<3; j++) cforce[i*8+j] += body[i]->facc[j]; - for (j=0; j<3; j++) cforce[i*8+4+j] += body[i]->tacc[j]; - } - // multiply cforce by stepsize - for (i=0; i < nb*8; i++) cforce[i] *= stepsize; - // add invM * cforce to the body velocity - for (i=0; i<nb; i++) { - dReal body_invMass = body[i]->invMass; - dReal *body_invI = invI + i*12; - for (j=0; j<3; j++) body[i]->lvel[j] += body_invMass * cforce[i*8+j]; - dMULTIPLYADD0_331 (body[i]->avel,body_invI,cforce+i*8+4); - } - - // update the position and orientation from the new linear/angular velocity - // (over the given timestep) -# ifdef TIMING - dTimerNow ("update position"); -# endif - for (i=0; i<nb; i++) moveAndRotateBody (body[i],stepsize); - -# ifdef COMPARE_METHODS - dReal *tmp_vnew = (dReal*) ALLOCA (nb*6*sizeof(dReal)); - for (i=0; i<nb; i++) { - for (j=0; j<3; j++) tmp_vnew[i*6+j] = body[i]->lvel[j]; - for (j=0; j<3; j++) tmp_vnew[i*6+3+j] = body[i]->avel[j]; - } - comparator.nextMatrix (tmp_vnew,nb*6,1,0,"vnew"); -# endif - -# ifdef TIMING - dTimerNow ("tidy up"); -# endif - - // zero all force accumulators - for (i=0; i<nb; i++) { - body[i]->facc[0] = 0; - body[i]->facc[1] = 0; - body[i]->facc[2] = 0; - body[i]->facc[3] = 0; - body[i]->tacc[0] = 0; - body[i]->tacc[1] = 0; - body[i]->tacc[2] = 0; - body[i]->tacc[3] = 0; - } - -# ifdef TIMING - dTimerEnd(); - if (m > 0) dTimerReport (stdout,1); -# endif -} - -//**************************************************************************** - -void dInternalStepIsland (dxWorld *world, dxBody * const *body, int nb, - dxJoint * const *joint, int nj, dReal stepsize) -{ -# ifndef COMPARE_METHODS - dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize); -# endif - -# ifdef COMPARE_METHODS - int i; - - // save body state - dxBody *state = (dxBody*) ALLOCA (nb*sizeof(dxBody)); - for (i=0; i<nb; i++) memcpy (state+i,body[i],sizeof(dxBody)); - - // take slow step - comparator.reset(); - dInternalStepIsland_x1 (world,body,nb,joint,nj,stepsize); - comparator.end(); - - // restore state - for (i=0; i<nb; i++) memcpy (body[i],state+i,sizeof(dxBody)); - - // take fast step - dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize); - comparator.end(); - - //comparator.dump(); - //_exit (1); -# endif -} diff --git a/extern/ode/dist/ode/src/step.h b/extern/ode/dist/ode/src/step.h deleted file mode 100644 index c825a11c1dc..00000000000 --- a/extern/ode/dist/ode/src/step.h +++ /dev/null @@ -1,37 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#ifndef _ODE_STEP_H_ -#define _ODE_STEP_H_ - -#include <ode/common.h> - - -void dInternalStepIsland (dxWorld *world, - dxBody * const *body, int nb, - dxJoint * const *joint, int nj, - dReal stepsize); - - - -#endif - diff --git a/extern/ode/dist/ode/src/testing.cpp b/extern/ode/dist/ode/src/testing.cpp deleted file mode 100644 index d55afc25257..00000000000 --- a/extern/ode/dist/ode/src/testing.cpp +++ /dev/null @@ -1,243 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#include <ode/config.h> -#include <ode/misc.h> -#include <ode/memory.h> -#include "testing.h" - -#ifdef dDOUBLE -static const dReal tol = 1.0e-9; -#else -static const dReal tol = 1.0e-5f; -#endif - - -// matrix header on the stack - -struct dMatrixComparison::dMatInfo { - int n,m; // size of matrix - char name[128]; // name of the matrix - dReal *data; // matrix data - int size; // size of `data' -}; - - - -dMatrixComparison::dMatrixComparison() -{ - afterfirst = 0; - index = 0; -} - - -dMatrixComparison::~dMatrixComparison() -{ - reset(); -} - - -dReal dMatrixComparison::nextMatrix (dReal *A, int n, int m, int lower_tri, - char *name, ...) -{ - if (A==0 || n < 1 || m < 1 || name==0) dDebug (0,"bad args to nextMatrix"); - int num = n*dPAD(m); - - if (afterfirst==0) { - dMatInfo *mi = (dMatInfo*) dAlloc (sizeof(dMatInfo)); - mi->n = n; - mi->m = m; - mi->size = num * sizeof(dReal); - mi->data = (dReal*) dAlloc (mi->size); - memcpy (mi->data,A,mi->size); - - va_list ap; - va_start (ap,name); - vsprintf (mi->name,name,ap); - if (strlen(mi->name) >= sizeof (mi->name)) dDebug (0,"name too long"); - - mat.push (mi); - return 0; - } - else { - if (lower_tri && n != m) - dDebug (0,"dMatrixComparison, lower triangular matrix must be square"); - if (index >= mat.size()) dDebug (0,"dMatrixComparison, too many matrices"); - dMatInfo *mp = mat[index]; - index++; - - dMatInfo mi; - va_list ap; - va_start (ap,name); - vsprintf (mi.name,name,ap); - if (strlen(mi.name) >= sizeof (mi.name)) dDebug (0,"name too long"); - - if (strcmp(mp->name,mi.name) != 0) - dDebug (0,"dMatrixComparison, name mismatch (\"%s\" and \"%s\")", - mp->name,mi.name); - if (mp->n != n || mp->m != m) - dDebug (0,"dMatrixComparison, size mismatch (%dx%d and %dx%d)", - mp->n,mp->m,n,m); - - dReal maxdiff; - if (lower_tri) { - maxdiff = dMaxDifferenceLowerTriangle (A,mp->data,n); - } - else { - maxdiff = dMaxDifference (A,mp->data,n,m); - } - if (maxdiff > tol) - dDebug (0,"dMatrixComparison, matrix error (size=%dx%d, name=\"%s\", " - "error=%.4e)",n,m,mi.name,maxdiff); - return maxdiff; - } -} - - -void dMatrixComparison::end() -{ - if (mat.size() <= 0) dDebug (0,"no matrices in sequence"); - afterfirst = 1; - index = 0; -} - - -void dMatrixComparison::reset() -{ - for (int i=0; i<mat.size(); i++) { - dFree (mat[i]->data,mat[i]->size); - dFree (mat[i],sizeof(dMatInfo)); - } - mat.setSize (0); - afterfirst = 0; - index = 0; -} - - -void dMatrixComparison::dump() -{ - for (int i=0; i<mat.size(); i++) - printf ("%d: %s (%dx%d)\n",i,mat[i]->name,mat[i]->n,mat[i]->m); -} - -//**************************************************************************** -// unit test - -#include <setjmp.h> - -static jmp_buf jump_buffer; - -static void myDebug (int num, const char *msg, va_list ap) -{ - // printf ("(Error %d: ",num); - // vprintf (msg,ap); - // printf (")\n"); - longjmp (jump_buffer,1); -} - - -extern "C" void dTestMatrixComparison() -{ - volatile int i; - printf ("dTestMatrixComparison()\n"); - dMessageFunction *orig_debug = dGetDebugHandler(); - - dMatrixComparison mc; - dReal A[50*50]; - - // make first sequence - unsigned long seed = dRandGetSeed(); - for (i=1; i<49; i++) { - dMakeRandomMatrix (A,i,i+1,1.0); - mc.nextMatrix (A,i,i+1,0,"A%d",i); - } - mc.end(); - - //mc.dump(); - - // test identical sequence - dSetDebugHandler (&myDebug); - dRandSetSeed (seed); - if (setjmp (jump_buffer)) { - printf ("\tFAILED (1)\n"); - } - else { - for (i=1; i<49; i++) { - dMakeRandomMatrix (A,i,i+1,1.0); - mc.nextMatrix (A,i,i+1,0,"A%d",i); - } - mc.end(); - printf ("\tpassed (1)\n"); - } - dSetDebugHandler (orig_debug); - - // test broken sequences (with matrix error) - dRandSetSeed (seed); - volatile int passcount = 0; - for (i=1; i<49; i++) { - if (setjmp (jump_buffer)) { - passcount++; - } - else { - dSetDebugHandler (&myDebug); - dMakeRandomMatrix (A,i,i+1,1.0); - A[(i-1)*dPAD(i+1)+i] += REAL(0.01); - mc.nextMatrix (A,i,i+1,0,"A%d",i); - dSetDebugHandler (orig_debug); - } - } - mc.end(); - printf ("\t%s (2)\n",(passcount == 48) ? "passed" : "FAILED"); - - // test broken sequences (with name error) - dRandSetSeed (seed); - passcount = 0; - for (i=1; i<49; i++) { - if (setjmp (jump_buffer)) { - passcount++; - } - else { - dSetDebugHandler (&myDebug); - dMakeRandomMatrix (A,i,i+1,1.0); - mc.nextMatrix (A,i,i+1,0,"B%d",i); - dSetDebugHandler (orig_debug); - } - } - mc.end(); - printf ("\t%s (3)\n",(passcount == 48) ? "passed" : "FAILED"); - - // test identical sequence again - dSetDebugHandler (&myDebug); - dRandSetSeed (seed); - if (setjmp (jump_buffer)) { - printf ("\tFAILED (4)\n"); - } - else { - for (i=1; i<49; i++) { - dMakeRandomMatrix (A,i,i+1,1.0); - mc.nextMatrix (A,i,i+1,0,"A%d",i); - } - mc.end(); - printf ("\tpassed (4)\n"); - } - dSetDebugHandler (orig_debug); -} diff --git a/extern/ode/dist/ode/src/testing.h b/extern/ode/dist/ode/src/testing.h deleted file mode 100644 index aa4acf119db..00000000000 --- a/extern/ode/dist/ode/src/testing.h +++ /dev/null @@ -1,66 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -/* stuff used for testing */ - -#ifndef _ODE_TESTING_H_ -#define _ODE_TESTING_H_ - -#include <ode/common.h> -#include "array.h" - - -// compare a sequence of named matrices/vectors, i.e. to make sure that two -// different pieces of code are giving the same results. - -class dMatrixComparison { - struct dMatInfo; - dArray<dMatInfo*> mat; - int afterfirst,index; - -public: - dMatrixComparison(); - ~dMatrixComparison(); - - dReal nextMatrix (dReal *A, int n, int m, int lower_tri, char *name, ...); - // add a new n*m matrix A to the sequence. the name of the matrix is given - // by the printf-style arguments (name,...). if this is the first sequence - // then this object will simply record the matrices and return 0. - // if this the second or subsequent sequence then this object will compare - // the matrices with the first sequence, and report any differences. - // the matrix error will be returned. if `lower_tri' is 1 then only the - // lower triangle of the matrix (including the diagonal) will be compared - // (the matrix must be square). - - void end(); - // end a sequence. - - void reset(); - // restarts the object, so the next sequence will be the first sequence. - - void dump(); - // print out info about all the matrices in the sequence -}; - - -#endif - diff --git a/extern/ode/dist/ode/src/timer.cpp b/extern/ode/dist/ode/src/timer.cpp deleted file mode 100644 index 87cb9f6f2fe..00000000000 --- a/extern/ode/dist/ode/src/timer.cpp +++ /dev/null @@ -1,397 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser General Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * General Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -/* - -TODO ----- - -* gettimeofday() and the pentium time stamp counter return the real time, - not the process time. fix this somehow! - -*/ - -#include <ode/common.h> -#include <ode/timer.h> - -// misc defines -#define ALLOCA dALLOCA16 - -//**************************************************************************** -// implementation for windows based on the multimedia performance counter. - -#ifdef WIN32 - -#include "windows.h" - -static inline void getClockCount (unsigned long cc[2]) -{ - LARGE_INTEGER a; - QueryPerformanceCounter (&a); - cc[0] = a.LowPart; - cc[1] = a.HighPart; -} - - -static inline void serialize() -{ -} - - -static inline double loadClockCount (unsigned long cc[2]) -{ - LARGE_INTEGER a; - a.LowPart = cc[0]; - a.HighPart = cc[1]; - return double(a.QuadPart); -} - - -double dTimerResolution() -{ - return 1.0/dTimerTicksPerSecond(); -} - - -double dTimerTicksPerSecond() -{ - static int query=0; - static double hz=0.0; - if (!query) { - LARGE_INTEGER a; - QueryPerformanceFrequency (&a); - hz = double(a.QuadPart); - query = 1; - } - return hz; -} - -#endif - -//**************************************************************************** -// implementation based on the pentium time stamp counter. the timer functions -// can be serializing or non-serializing. serializing will ensure that all -// instructions have executed and data has been written back before the cpu -// time stamp counter is read. the CPUID instruction is used to serialize. - -#if defined(PENTIUM) && !defined(WIN32) - -// we need to know the clock rate so that the timing function can report -// accurate times. this number only needs to be set accurately if we're -// doing performance tests and care about real-world time numbers - otherwise, -// just ignore this. i have not worked out how to determine this number -// automatically yet. - -#define PENTIUM_HZ (500e6) - - -static inline void getClockCount (unsigned long cc[2]) -{ - asm volatile ("\n\ - rdtsc\n\ - movl %%eax,(%%esi)\n\ - movl %%edx,4(%%esi)" - : : "S" (cc) : "%eax","%edx","cc","memory"); -} - - -static inline void serialize() -{ - asm volatile ("\n\ - mov $0,%%eax\n\ - cpuid" - : : : "%eax","%ebx","%ecx","%edx","cc","memory"); -} - - -static inline double loadClockCount (unsigned long a[2]) -{ - double ret; - asm volatile ("fildll %1; fstpl %0" : "=m" (ret) : "m" (a[0]) : - "cc","memory"); - return ret; -} - - -double dTimerResolution() -{ - return 1.0/PENTIUM_HZ; -} - - -double dTimerTicksPerSecond() -{ - return PENTIUM_HZ; -} - -#endif - -//**************************************************************************** -// otherwise, do the implementation based on gettimeofday(). - -#if !defined(PENTIUM) && !defined(WIN32) - -#ifndef macintosh - -#include <sys/time.h> -#include <unistd.h> - - -static inline void getClockCount (unsigned long cc[2]) -{ - struct timeval tv; - gettimeofday (&tv,0); - cc[0] = tv.tv_usec; - cc[1] = tv.tv_sec; -} - -#else // macintosh - -#include <MacTypes.h> -#include <Timer.h> - -static inline void getClockCount (unsigned long cc[2]) -{ - UnsignedWide ms; - Microseconds (&ms); - cc[1] = ms.lo / 1000000; - cc[0] = ms.lo - ( cc[1] * 1000000 ); -} - -#endif - - -static inline void serialize() -{ -} - - -static inline double loadClockCount (unsigned long a[2]) -{ - return a[1]*1.0e6 + a[0]; -} - - -double dTimerResolution() -{ - unsigned long cc1[2],cc2[2]; - getClockCount (cc1); - do { - getClockCount (cc2); - } - while (cc1[0]==cc2[0] && cc1[1]==cc2[1]); - do { - getClockCount (cc1); - } - while (cc1[0]==cc2[0] && cc1[1]==cc2[1]); - double t1 = loadClockCount (cc1); - double t2 = loadClockCount (cc2); - return (t1-t2) / dTimerTicksPerSecond(); -} - - -double dTimerTicksPerSecond() -{ - return 1000000; -} - -#endif - -//**************************************************************************** -// stop watches - -void dStopwatchReset (dStopwatch *s) -{ - s->time = 0; - s->cc[0] = 0; - s->cc[1] = 0; -} - - -void dStopwatchStart (dStopwatch *s) -{ - serialize(); - getClockCount (s->cc); -} - - -void dStopwatchStop (dStopwatch *s) -{ - unsigned long cc[2]; - serialize(); - getClockCount (cc); - double t1 = loadClockCount (s->cc); - double t2 = loadClockCount (cc); - s->time += t2-t1; -} - - -double dStopwatchTime (dStopwatch *s) -{ - return s->time / dTimerTicksPerSecond(); -} - -//**************************************************************************** -// code timers - -// maximum number of events to record -#define MAXNUM 100 - -static int num = 0; // number of entries used in event array -static struct { - unsigned long cc[2]; // clock counts - double total_t; // total clocks used in this slot. - double total_p; // total percentage points used in this slot. - int count; // number of times this slot has been updated. - char *description; // pointer to static string -} event[MAXNUM]; - - -// make sure all slot totals and counts reset to 0 at start - -static void initSlots() -{ - static int initialized=0; - if (!initialized) { - for (int i=0; i<MAXNUM; i++) { - event[i].count = 0; - event[i].total_t = 0; - event[i].total_p = 0; - } - initialized = 1; - } -} - - -void dTimerStart (const char *description) -{ - initSlots(); - event[0].description = const_cast<char*> (description); - num = 1; - serialize(); - getClockCount (event[0].cc); -} - - -void dTimerNow (const char *description) -{ - if (num < MAXNUM) { - // do not serialize - getClockCount (event[num].cc); - event[num].description = const_cast<char*> (description); - num++; - } -} - - -void dTimerEnd() -{ - if (num < MAXNUM) { - serialize(); - getClockCount (event[num].cc); - event[num].description = "TOTAL"; - num++; - } -} - -//**************************************************************************** -// print report - -static void fprintDoubleWithPrefix (FILE *f, double a, char *fmt) -{ - if (a >= 0.999999) { - fprintf (f,fmt,a); - return; - } - a *= 1000.0; - if (a >= 0.999999) { - fprintf (f,fmt,a); - fprintf (f,"m"); - return; - } - a *= 1000.0; - if (a >= 0.999999) { - fprintf (f,fmt,a); - fprintf (f,"u"); - return; - } - a *= 1000.0; - fprintf (f,fmt,a); - fprintf (f,"n"); -} - - -void dTimerReport (FILE *fout, int average) -{ - int i,maxl; - double ccunit = 1.0/dTimerTicksPerSecond(); - fprintf (fout,"\nTimer Report ("); - fprintDoubleWithPrefix (fout,ccunit,"%.2f "); - fprintf (fout,"s resolution)\n------------\n"); - if (num < 1) return; - - // get maximum description length - maxl = 0; - for (i=0; i<num; i++) { - int l = strlen (event[i].description); - if (l > maxl) maxl = l; - } - - // calculate total time - double t1 = loadClockCount (event[0].cc); - double t2 = loadClockCount (event[num-1].cc); - double total = t2 - t1; - if (total <= 0) total = 1; - - // compute time difference for all slots except the last one. update totals - double *times = (double*) ALLOCA (num * sizeof(double)); - for (i=0; i < (num-1); i++) { - double t1 = loadClockCount (event[i].cc); - double t2 = loadClockCount (event[i+1].cc); - times[i] = t2 - t1; - event[i].count++; - event[i].total_t += times[i]; - event[i].total_p += times[i]/total * 100.0; - } - - // print report (with optional averages) - for (i=0; i<num; i++) { - double t,p; - if (i < (num-1)) { - t = times[i]; - p = t/total * 100.0; - } - else { - t = total; - p = 100.0; - } - fprintf (fout,"%-*s %7.2fms %6.2f%%",maxl,event[i].description, - t*ccunit * 1000.0, p); - if (average && i < (num-1)) { - fprintf (fout," (avg %7.2fms %6.2f%%)", - (event[i].total_t / event[i].count)*ccunit * 1000.0, - event[i].total_p / event[i].count); - } - fprintf (fout,"\n"); - } - fprintf (fout,"\n"); -} diff --git a/extern/ode/dist/tools/build4 b/extern/ode/dist/tools/build4 deleted file mode 100755 index 49831e2df35..00000000000 --- a/extern/ode/dist/tools/build4 +++ /dev/null @@ -1,42 +0,0 @@ -#!/bin/sh -# -# build all four precision/release configurations and log the build messages -# (used for debugging). - -PLATFORM=unix-gcc -SETTINGS=config/user-settings - -if [ ! -f ode/src/ode.cpp ]; then - echo "run this from the ODE root directory" - exit 1 -fi - -function build() { -echo -e "$PRECISION $MODE\n\n" >> BUILD_LOG -cat <<END > $SETTINGS -PLATFORM=$PLATFORM -PRECISION=$PRECISION -BUILD=$MODE -END -make clean -make >> BUILD_LOG 2>&1 -echo -e "\n\n---------------------------------------------\n\n" >> BUILD_LOG -} - -echo > BUILD_LOG - -PRECISION=SINGLE -MODE=debug -build -PRECISION=SINGLE -MODE=release -build -PRECISION=DOUBLE -MODE=debug -build -PRECISION=DOUBLE -MODE=release -build - -make clean -rm -f $SETTINGS diff --git a/extern/ode/dist/tools/build4.bat b/extern/ode/dist/tools/build4.bat deleted file mode 100755 index c87e9a9e2ba..00000000000 --- a/extern/ode/dist/tools/build4.bat +++ /dev/null @@ -1,43 +0,0 @@ -@echo off -rem build all four precision/release configurations and log the build messages -rem (used for debugging). - -setlocal - -set PLATFORM=cygwin -set SETTINGS=config\user-settings - -echo SINGLE debug > BUILD_LOG -echo PLATFORM=%PLATFORM%> %SETTINGS% -echo PRECISION=SINGLE>> %SETTINGS% -echo BUILD=debug>> %SETTINGS% -make clean -make >> BUILD_LOG -echo --------------------------------------------- >> BUILD_LOG - -echo DOUBLE debug >> BUILD_LOG -echo PLATFORM=%PLATFORM%> %SETTINGS% -echo PRECISION=DOUBLE>> %SETTINGS% -echo BUILD=debug>> %SETTINGS% -make clean -make >> BUILD_LOG -echo --------------------------------------------- >> BUILD_LOG - -echo SINGLE release >> BUILD_LOG -echo PLATFORM=%PLATFORM%> %SETTINGS% -echo PRECISION=SINGLE>> %SETTINGS% -echo BUILD=release>> %SETTINGS% -make clean -make >> BUILD_LOG -echo --------------------------------------------- >> BUILD_LOG - -echo DOUBLE release >> BUILD_LOG -echo PLATFORM=%PLATFORM%> %SETTINGS% -echo PRECISION=DOUBLE>> %SETTINGS% -echo BUILD=release>> %SETTINGS% -make clean -make >> BUILD_LOG -echo --------------------------------------------- >> BUILD_LOG - -make clean -del %SETTINGS% diff --git a/extern/ode/dist/tools/make_distribution b/extern/ode/dist/tools/make_distribution deleted file mode 100755 index ed6d52bcbfd..00000000000 --- a/extern/ode/dist/tools/make_distribution +++ /dev/null @@ -1,45 +0,0 @@ -#!/bin/sh - -VER=0.03 -# VER=`date +%y%m%d` - -if [ ! -f ode/src/ode.cpp ]; then - echo "run this from the ODE root directory" - exit 1 -fi - -ODE_DIR=`pwd` - -cd /tmp -if [ -d /tmp/ode-$VER ]; then - echo "remove /tmp/ode-$VER first" - exit 1 -fi - -mkdir /tmp/ode-$VER -cp -av $ODE_DIR/* /tmp/ode-$VER -find /tmp/ode-$VER -type d -name CVS -exec rm -rf {} \; -print -find /tmp/ode-$VER -type f -name *~ -exec rm -f {} \; -print -rmdir /tmp/ode-$VER/build - -cd /tmp/ode-$VER -make clean -cp config/user-settings.example config/user-settings - -cd ode/doc -./doccer ode.doc > ode.html - -cd /tmp/ode-$VER -echo -e "\n\nMake any modifications you want, then exit the shell:" -bash - -cd /tmp -tar cfvz ode-$VER.tgz ode-$VER -rm -rf /tmp/ode-$VER - -echo -e "\ntype <return> to exit or 'c' to copy to q12" -read Q -if [ $Q ]; then - echo copying... - scp1 ode-$VER.tgz q12.org:~/q12/ode/release/ -fi diff --git a/extern/ode/dist/tools/process_deps b/extern/ode/dist/tools/process_deps deleted file mode 100755 index 9b95ddac382..00000000000 --- a/extern/ode/dist/tools/process_deps +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/perl - -$a = join ('',<STDIN>); -$a =~ s/\\\n/ /g; # join continued lines -$a =~ s/(^\S+:)/$ARGV[0]$1/gm; # put prefix in front of rules -$a =~ s/\s+\/\S+/ /g; # remove absolute path dependencies -$a =~ s/\s+\n/\n/g; # remove whitespace at end of lines -$a =~ s/[ \t]+/ /g; # clean up interior whitespace -$a =~ s/ / \\\n /g; # put back line continuations - -print $a; |