diff options
| author | Norman Lin <nlin@nlin.net> | 2002-10-18 19:02:02 +0400 |
|---|---|---|
| committer | Norman Lin <nlin@nlin.net> | 2002-10-18 19:02:02 +0400 |
| commit | bdad961ce365f6a57f12832b5ba763525ac164fb (patch) | |
| tree | 920c54fc077f20b7ac5632e1dfa3a2fc5194e7d8 | |
| parent | 1b1596178640ac4b0b75fd4b97ff08399a4c075d (diff) | |
checkin of ODE library. Do not modify the ODE source code; instead, follow the
development of ode at http://q12.org and periodically copy the q12.org ODE
sourcecode into this tree to update the Blender ODE.
This ODE has not been changed from q12.org and is provided here merely as a
convenience to Blender developers.
97 files changed, 21829 insertions, 0 deletions
diff --git a/extern/ode/dist/INSTALL b/extern/ode/dist/INSTALL new file mode 100644 index 00000000000..f82285db1b2 --- /dev/null +++ b/extern/ode/dist/INSTALL @@ -0,0 +1,44 @@ + +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 new file mode 100644 index 00000000000..05929239487 --- /dev/null +++ b/extern/ode/dist/LICENSE-BSD.TXT @@ -0,0 +1,34 @@ + +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 new file mode 100644 index 00000000000..cfe59bcadb8 --- /dev/null +++ b/extern/ode/dist/LICENSE.TXT @@ -0,0 +1,502 @@ + 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 new file mode 100644 index 00000000000..54ae6833982 --- /dev/null +++ b/extern/ode/dist/Makefile @@ -0,0 +1,280 @@ +######################################################################### +# # +# 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. + +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 new file mode 100644 index 00000000000..ad11f01353f --- /dev/null +++ b/extern/ode/dist/Makefile.deps @@ -0,0 +1,456 @@ +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 new file mode 100644 index 00000000000..6b151d1a27b --- /dev/null +++ b/extern/ode/dist/README @@ -0,0 +1,30 @@ +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 new file mode 100644 index 00000000000..64d97624a55 --- /dev/null +++ b/extern/ode/dist/README_BLENDER @@ -0,0 +1,18 @@ +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 new file mode 100644 index 00000000000..0fa18062a4e --- /dev/null +++ b/extern/ode/dist/config/README @@ -0,0 +1,41 @@ + +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 new file mode 100644 index 00000000000..de23b71a29f --- /dev/null +++ b/extern/ode/dist/config/makefile.cygwin @@ -0,0 +1,28 @@ +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 new file mode 100644 index 00000000000..4b18fb6bcdc --- /dev/null +++ b/extern/ode/dist/config/makefile.mingw @@ -0,0 +1,28 @@ +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 new file mode 100644 index 00000000000..9d4da0bf912 --- /dev/null +++ b/extern/ode/dist/config/makefile.msvc @@ -0,0 +1,27 @@ +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 new file mode 100644 index 00000000000..fe495893616 --- /dev/null +++ b/extern/ode/dist/config/makefile.msvc-dll @@ -0,0 +1,29 @@ +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 new file mode 100644 index 00000000000..cb883e3895c --- /dev/null +++ b/extern/ode/dist/config/makefile.osx @@ -0,0 +1,26 @@ +THIS_DIR=./ +DEL_CMD=rm -f +CC=cc +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 diff --git a/extern/ode/dist/config/makefile.unix-gcc b/extern/ode/dist/config/makefile.unix-gcc new file mode 100644 index 00000000000..b9d07632353 --- /dev/null +++ b/extern/ode/dist/config/makefile.unix-gcc @@ -0,0 +1,29 @@ +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 new file mode 100644 index 00000000000..f435e1a0db8 --- /dev/null +++ b/extern/ode/dist/config/makefile.unix-generic @@ -0,0 +1,24 @@ +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 new file mode 100644 index 00000000000..11258ab9aa8 --- /dev/null +++ b/extern/ode/dist/config/msvcdefs.def @@ -0,0 +1,228 @@ +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 new file mode 100644 index 00000000000..d632eba9678 --- /dev/null +++ b/extern/ode/dist/config/user-settings @@ -0,0 +1,31 @@ +# 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 new file mode 100644 index 00000000000..0b0d480a25a --- /dev/null +++ b/extern/ode/dist/config/user-settings.example @@ -0,0 +1,31 @@ +# 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 new file mode 100644 index 00000000000..8129716881d --- /dev/null +++ b/extern/ode/dist/configurator.c @@ -0,0 +1,437 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..aaedfcc38fe --- /dev/null +++ b/extern/ode/dist/include/ode/README @@ -0,0 +1,18 @@ + +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 new file mode 100644 index 00000000000..705d222eb50 --- /dev/null +++ b/extern/ode/dist/include/ode/common.h @@ -0,0 +1,306 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..926d77f6c43 --- /dev/null +++ b/extern/ode/dist/include/ode/contact.h @@ -0,0 +1,90 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..700bf44dabd --- /dev/null +++ b/extern/ode/dist/include/ode/error.h @@ -0,0 +1,63 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..ec0aafe4ec1 --- /dev/null +++ b/extern/ode/dist/include/ode/geom.h @@ -0,0 +1,152 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..53e437437c8 --- /dev/null +++ b/extern/ode/dist/include/ode/mass.h @@ -0,0 +1,97 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..75218fd3ee9 --- /dev/null +++ b/extern/ode/dist/include/ode/matrix.h @@ -0,0 +1,194 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..476a05a2de1 --- /dev/null +++ b/extern/ode/dist/include/ode/memory.h @@ -0,0 +1,63 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..9ceca3077e9 --- /dev/null +++ b/extern/ode/dist/include/ode/misc.h @@ -0,0 +1,85 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..9531c639f05 --- /dev/null +++ b/extern/ode/dist/include/ode/objects.h @@ -0,0 +1,201 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..f0ee944f5a8 --- /dev/null +++ b/extern/ode/dist/include/ode/ode.h @@ -0,0 +1,44 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..3eaa9732cf2 --- /dev/null +++ b/extern/ode/dist/include/ode/odecpp.h @@ -0,0 +1,796 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..49e7d7f3cfb --- /dev/null +++ b/extern/ode/dist/include/ode/odecpp_old.h @@ -0,0 +1,316 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..070c1185af8 --- /dev/null +++ b/extern/ode/dist/include/ode/odemath.h @@ -0,0 +1,216 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..26df94d4cdc --- /dev/null +++ b/extern/ode/dist/include/ode/rotation.h @@ -0,0 +1,64 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..21107fe6a9a --- /dev/null +++ b/extern/ode/dist/include/ode/space.h @@ -0,0 +1,77 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..dcae5b5141c --- /dev/null +++ b/extern/ode/dist/include/ode/timer.h @@ -0,0 +1,76 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..dd4596f9935 --- /dev/null +++ b/extern/ode/dist/ode/README @@ -0,0 +1,158 @@ +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 new file mode 100644 index 00000000000..09b49274da8 --- /dev/null +++ b/extern/ode/dist/ode/fbuild/BuildDot @@ -0,0 +1,148 @@ +#!/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 new file mode 100644 index 00000000000..fd74faf18df --- /dev/null +++ b/extern/ode/dist/ode/fbuild/BuildLDLT @@ -0,0 +1,654 @@ +#!/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 new file mode 100644 index 00000000000..f6b117708c5 --- /dev/null +++ b/extern/ode/dist/ode/fbuild/BuildMultidot @@ -0,0 +1,174 @@ +#!/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 new file mode 100644 index 00000000000..b0828ff5ec1 --- /dev/null +++ b/extern/ode/dist/ode/fbuild/BuildUtil @@ -0,0 +1,99 @@ +#!/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 new file mode 100644 index 00000000000..86f5cd4417b --- /dev/null +++ b/extern/ode/dist/ode/fbuild/Dependencies @@ -0,0 +1,16 @@ +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 new file mode 100644 index 00000000000..f988c3739af --- /dev/null +++ b/extern/ode/dist/ode/fbuild/Makefile @@ -0,0 +1,77 @@ +######################################################################### +# # +# 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 new file mode 100644 index 00000000000..1dc178262aa --- /dev/null +++ b/extern/ode/dist/ode/fbuild/OptimizeDot @@ -0,0 +1,71 @@ +#!/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 new file mode 100644 index 00000000000..612633e00c2 --- /dev/null +++ b/extern/ode/dist/ode/fbuild/OptimizeLDLT @@ -0,0 +1,91 @@ +#!/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 new file mode 100644 index 00000000000..bba6b495f2c --- /dev/null +++ b/extern/ode/dist/ode/fbuild/OptimizeLSolve @@ -0,0 +1,76 @@ +#!/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 new file mode 100644 index 00000000000..a10109e2d2c --- /dev/null +++ b/extern/ode/dist/ode/fbuild/OptimizeLTSolve @@ -0,0 +1,76 @@ +#!/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 new file mode 100644 index 00000000000..f2b54383312 --- /dev/null +++ b/extern/ode/dist/ode/fbuild/OptimizeMultidot @@ -0,0 +1,73 @@ +#!/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 new file mode 100644 index 00000000000..2b882fcba65 --- /dev/null +++ b/extern/ode/dist/ode/fbuild/OptimizeUtil @@ -0,0 +1,86 @@ +#!/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 new file mode 100644 index 00000000000..e58f279f7d7 --- /dev/null +++ b/extern/ode/dist/ode/fbuild/ParametersD.example @@ -0,0 +1,32 @@ +######################################################################### +# # +# 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 new file mode 100644 index 00000000000..9881b09ad81 --- /dev/null +++ b/extern/ode/dist/ode/fbuild/ParametersF.example @@ -0,0 +1,30 @@ +######################################################################### +# # +# 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 new file mode 100644 index 00000000000..bb81d6b83f1 --- /dev/null +++ b/extern/ode/dist/ode/fbuild/ParametersM.example @@ -0,0 +1,32 @@ +######################################################################### +# # +# 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 new file mode 100644 index 00000000000..29c9a91fc16 --- /dev/null +++ b/extern/ode/dist/ode/fbuild/ParametersS.example @@ -0,0 +1,30 @@ +######################################################################### +# # +# 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 new file mode 100644 index 00000000000..3aa92a31dd2 --- /dev/null +++ b/extern/ode/dist/ode/fbuild/ParametersT.example @@ -0,0 +1,30 @@ +######################################################################### +# # +# 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 new file mode 100644 index 00000000000..bcf7f5aa8b5 --- /dev/null +++ b/extern/ode/dist/ode/fbuild/README @@ -0,0 +1,41 @@ + +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 new file mode 100644 index 00000000000..19ae2d1c94e --- /dev/null +++ b/extern/ode/dist/ode/fbuild/ldlt.m @@ -0,0 +1,26 @@ +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 new file mode 100644 index 00000000000..62aec32e509 --- /dev/null +++ b/extern/ode/dist/ode/fbuild/test_dot.cpp @@ -0,0 +1,124 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..3b795b9ee7f --- /dev/null +++ b/extern/ode/dist/ode/fbuild/test_ldlt.cpp @@ -0,0 +1,299 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..0e961152fed --- /dev/null +++ b/extern/ode/dist/ode/fbuild/test_multidot.cpp @@ -0,0 +1,144 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..cbb1a6ed557 --- /dev/null +++ b/extern/ode/dist/ode/src/array.cpp @@ -0,0 +1,80 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..ae5caf2e40f --- /dev/null +++ b/extern/ode/dist/ode/src/array.h @@ -0,0 +1,135 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..9b33db55f0c --- /dev/null +++ b/extern/ode/dist/ode/src/error.cpp @@ -0,0 +1,172 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..148d2dd9e17 --- /dev/null +++ b/extern/ode/dist/ode/src/fastdot.c @@ -0,0 +1,30 @@ +/* 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 new file mode 100644 index 00000000000..df2ea6ec229 --- /dev/null +++ b/extern/ode/dist/ode/src/fastldlt.c @@ -0,0 +1,381 @@ +/* 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 new file mode 100644 index 00000000000..0ae99d62d0b --- /dev/null +++ b/extern/ode/dist/ode/src/fastlsolve.c @@ -0,0 +1,298 @@ +/* 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 new file mode 100644 index 00000000000..eb950f6076a --- /dev/null +++ b/extern/ode/dist/ode/src/fastltsolve.c @@ -0,0 +1,199 @@ +/* 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 new file mode 100644 index 00000000000..1818814a791 --- /dev/null +++ b/extern/ode/dist/ode/src/geom.cpp @@ -0,0 +1,2207 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..f483bc6c323 --- /dev/null +++ b/extern/ode/dist/ode/src/geom_internal.h @@ -0,0 +1,84 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..74e4c34cc71 --- /dev/null +++ b/extern/ode/dist/ode/src/joint.cpp @@ -0,0 +1,2160 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..5874b80c491 --- /dev/null +++ b/extern/ode/dist/ode/src/joint.h @@ -0,0 +1,261 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..dba2d3b949b --- /dev/null +++ b/extern/ode/dist/ode/src/lcp.cpp @@ -0,0 +1,1455 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..adb66fe70da --- /dev/null +++ b/extern/ode/dist/ode/src/lcp.h @@ -0,0 +1,58 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..9c1aae2033f --- /dev/null +++ b/extern/ode/dist/ode/src/mass.cpp @@ -0,0 +1,261 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..6e635dcc994 --- /dev/null +++ b/extern/ode/dist/ode/src/mat.cpp @@ -0,0 +1,230 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..2814a01bfcc --- /dev/null +++ b/extern/ode/dist/ode/src/mat.h @@ -0,0 +1,71 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..16afe915dd6 --- /dev/null +++ b/extern/ode/dist/ode/src/matrix.cpp @@ -0,0 +1,358 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..df61f97375f --- /dev/null +++ b/extern/ode/dist/ode/src/memory.cpp @@ -0,0 +1,278 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..bdc1579d5aa --- /dev/null +++ b/extern/ode/dist/ode/src/misc.cpp @@ -0,0 +1,147 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..4e3919dc829 --- /dev/null +++ b/extern/ode/dist/ode/src/objects.h @@ -0,0 +1,91 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..a6b9d36fbb4 --- /dev/null +++ b/extern/ode/dist/ode/src/obstack.cpp @@ -0,0 +1,130 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..fd283fed484 --- /dev/null +++ b/extern/ode/dist/ode/src/obstack.h @@ -0,0 +1,68 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..1a14e3292a0 --- /dev/null +++ b/extern/ode/dist/ode/src/ode.cpp @@ -0,0 +1,1341 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..cd6dbc855fe --- /dev/null +++ b/extern/ode/dist/ode/src/odemath.cpp @@ -0,0 +1,173 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..22b9fb13820 --- /dev/null +++ b/extern/ode/dist/ode/src/rotation.cpp @@ -0,0 +1,283 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..ca8c11f1f1b --- /dev/null +++ b/extern/ode/dist/ode/src/scrapbook.cpp @@ -0,0 +1,270 @@ + +/* + +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 new file mode 100644 index 00000000000..0c656573918 --- /dev/null +++ b/extern/ode/dist/ode/src/space.cpp @@ -0,0 +1,621 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..e062f92b54f --- /dev/null +++ b/extern/ode/dist/ode/src/stack.cpp @@ -0,0 +1,114 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..5afff41a12e --- /dev/null +++ b/extern/ode/dist/ode/src/stack.h @@ -0,0 +1,138 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..16f77112ad2 --- /dev/null +++ b/extern/ode/dist/ode/src/step.cpp @@ -0,0 +1,1085 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..494725387ad --- /dev/null +++ b/extern/ode/dist/ode/src/step.h @@ -0,0 +1,36 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..d55afc25257 --- /dev/null +++ b/extern/ode/dist/ode/src/testing.cpp @@ -0,0 +1,243 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..4d19ff329ad --- /dev/null +++ b/extern/ode/dist/ode/src/testing.h @@ -0,0 +1,65 @@ +/************************************************************************* + * * + * 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 new file mode 100644 index 00000000000..09e9c5f59b0 --- /dev/null +++ b/extern/ode/dist/ode/src/timer.cpp @@ -0,0 +1,397 @@ +/************************************************************************* + * * + * 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 (" + rdtsc + movl %%eax,(%%esi) + movl %%edx,4(%%esi)" + : : "S" (cc) : "%eax","%edx","cc","memory"); +} + + +static inline void serialize() +{ + asm volatile (" + mov $0,%%eax + 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 new file mode 100755 index 00000000000..49831e2df35 --- /dev/null +++ b/extern/ode/dist/tools/build4 @@ -0,0 +1,42 @@ +#!/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 new file mode 100755 index 00000000000..c87e9a9e2ba --- /dev/null +++ b/extern/ode/dist/tools/build4.bat @@ -0,0 +1,43 @@ +@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 new file mode 100755 index 00000000000..ed6d52bcbfd --- /dev/null +++ b/extern/ode/dist/tools/make_distribution @@ -0,0 +1,45 @@ +#!/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 new file mode 100755 index 00000000000..9b95ddac382 --- /dev/null +++ b/extern/ode/dist/tools/process_deps @@ -0,0 +1,11 @@ +#!/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; |