remove ODE, build systems and BGE are ignoring it already.
This commit is contained in:
Campbell Barton
111
extern/ode/Makefile
vendored
111
extern/ode/Makefile
vendored
@@ -1,111 +0,0 @@
|
||||
# -*- mode: gnumakefile; tab-width: 8; indent-tabs-mode: t; -*-
|
||||
# vim: tabstop=8
|
||||
#
|
||||
# $Id$
|
||||
#
|
||||
# ***** BEGIN GPL LICENSE BLOCK *****
|
||||
#
|
||||
# This program is free software; you can redistribute it and/or
|
||||
# modify it under the terms of the GNU General Public License
|
||||
# as published by the Free Software Foundation; either version 2
|
||||
# of the License, or (at your option) any later version.
|
||||
#
|
||||
# This program 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 General Public License for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with this program; if not, write to the Free Software Foundation,
|
||||
# Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
|
||||
#
|
||||
# The Original Code is Copyright (C) 2002 by Hans Lambermont
|
||||
# All rights reserved.
|
||||
#
|
||||
# The Original Code is: all of this file.
|
||||
#
|
||||
# Contributor(s): GSR
|
||||
#
|
||||
# ***** END GPL LICENSE BLOCK *****
|
||||
|
||||
include nan_definitions.mk
|
||||
|
||||
DISTDIR = dist
|
||||
CP = ../../intern/tools/cpifdiff.sh
|
||||
USERSETTINGS = ./dist/config/user-settings
|
||||
TEMPSETTINGS = ./user-settings
|
||||
|
||||
all:
|
||||
[ -d $(DISTDIR)/lib ] || mkdir $(DISTDIR)/lib
|
||||
# prepare settings for patching, clean in case of interruption
|
||||
ifeq ($(OS),$(findstring $(OS), "darwin windows"))
|
||||
[ ! -f $(TEMPSETTINGS) ] || mv $(TEMPSETTINGS) $(USERSETTINGS)
|
||||
cp $(USERSETTINGS) $(TEMPSETTINGS)
|
||||
endif
|
||||
ifeq ($(OS),freebsd)
|
||||
(grep FreeBSD $(DISTDIR)/Makefile >/dev/null ; \
|
||||
[ $$? -eq 0 ] || patch < patchfile.FreeBSD )
|
||||
endif
|
||||
ifeq ($(OS),darwin)
|
||||
cat $(TEMPSETTINGS) | sed s/unix-gcc/osx/ > $(USERSETTINGS)
|
||||
endif
|
||||
ifeq ($(OS),windows)
|
||||
# compile with MSVC
|
||||
cat $(TEMPSETTINGS) | sed s/unix-gcc/msvc/ > $(USERSETTINGS)
|
||||
env PATH=".:$(PATH)" $(MAKE) -C $(DISTDIR)
|
||||
endif
|
||||
ifeq ($(OS),$(findstring $(OS), "freebsd linux darwin"))
|
||||
$(MAKE) -C $(DISTDIR)
|
||||
endif
|
||||
# restore settings
|
||||
ifeq ($(OS),$(findstring $(OS), "darwin windows"))
|
||||
mv $(TEMPSETTINGS) $(USERSETTINGS)
|
||||
endif
|
||||
|
||||
debug:: install
|
||||
|
||||
install: all
|
||||
ifeq ($(OS),$(findstring $(OS), "freebsd linux darwin"))
|
||||
[ -d $(LCGDIR) ] || mkdir $(LCGDIR)
|
||||
[ -d $(NAN_ODE) ] || mkdir $(NAN_ODE)
|
||||
[ -d $(NAN_ODE)/include ] || mkdir $(NAN_ODE)/include
|
||||
[ -d $(NAN_ODE)/include/ode ] || mkdir $(NAN_ODE)/include/ode
|
||||
[ -d $(NAN_ODE)/lib ] || mkdir $(NAN_ODE)/lib
|
||||
[ -d $(NAN_ODE)/ode ] || mkdir $(NAN_ODE)/ode
|
||||
[ -d $(NAN_ODE)/ode/src ] || mkdir $(NAN_ODE)/ode/src
|
||||
@$(CP) $(DISTDIR)/lib/libode.a $(NAN_ODE)/lib/
|
||||
@$(CP) $(DISTDIR)/include/ode/*.h $(NAN_ODE)/include/ode/
|
||||
@$(CP) $(DISTDIR)/ode/src/array.h $(NAN_ODE)/ode/src/
|
||||
@$(CP) $(DISTDIR)/ode/src/joint.h $(NAN_ODE)/ode/src/
|
||||
@$(CP) $(DISTDIR)/ode/src/objects.h $(NAN_ODE)/ode/src/
|
||||
@$(CP) $(DISTDIR)/ode/src/obstack.h $(NAN_ODE)/ode/src/
|
||||
ifeq ($(OS),darwin)
|
||||
ranlib $(NAN_ODE)/lib/libode.a
|
||||
endif
|
||||
endif
|
||||
ifeq ($(OS),windows)
|
||||
@echo "====> $(MAKE) $@ in $(SOURCEDIR)"
|
||||
[ -d $(LCGDIR) ] || mkdir $(LCGDIR)
|
||||
[ -d $(NAN_ODE) ] || mkdir $(NAN_ODE)
|
||||
[ -d $(NAN_ODE)/include ] || mkdir $(NAN_ODE)/include
|
||||
[ -d $(NAN_ODE)/include/ode ] || mkdir $(NAN_ODE)/include/ode
|
||||
[ -d $(NAN_ODE)/lib ] || mkdir $(NAN_ODE)/lib
|
||||
[ -d $(NAN_ODE)/ode ] || mkdir $(NAN_ODE)/ode
|
||||
[ -d $(NAN_ODE)/ode/src ] || mkdir $(NAN_ODE)/ode/src
|
||||
cp $(DISTDIR)/lib/ode.lib $(NAN_ODE)/lib/libode.a
|
||||
cp $(DISTDIR)/include/ode/*.h $(NAN_ODE)/include/ode/
|
||||
cp $(DISTDIR)/ode/src/array.h $(NAN_ODE)/ode/src/
|
||||
cp $(DISTDIR)/ode/src/joint.h $(NAN_ODE)/ode/src/
|
||||
cp $(DISTDIR)/ode/src/objects.h $(NAN_ODE)/ode/src/
|
||||
cp $(DISTDIR)/ode/src/obstack.h $(NAN_ODE)/ode/src/
|
||||
endif
|
||||
|
||||
clean:
|
||||
ifeq ($(OS),$(findstring $(OS), "freebsd linux darwin"))
|
||||
[ ! -f dist/Makefile ] || $(MAKE) -C dist clean
|
||||
endif
|
||||
ifeq ($(OS),freebsd)
|
||||
(grep FreeBSD $(DISTDIR)/Makefile >/dev/null ; \
|
||||
[ $$? -ne 0 ] || patch -R < patchfile.FreeBSD )
|
||||
endif
|
||||
|
||||
44
extern/ode/dist/INSTALL
vendored
44
extern/ode/dist/INSTALL
vendored
@@ -1,44 +0,0 @@
|
||||
|
||||
here are the steps to buid ODE:
|
||||
|
||||
(1) get the GNU 'make' tool. many unix platforms come with this, although
|
||||
sometimes it is called 'gmake'. i have provided a version of GNU make
|
||||
for windows at: http://q12.org/ode/bin/make.exe
|
||||
|
||||
(2) edit the settings in the file config/user-settings. the list of supported
|
||||
platforms is given in that file.
|
||||
|
||||
(3) run 'make' to configure and build ODE and the graphical test programs.
|
||||
to build parts of ODE the make targets are:
|
||||
|
||||
make configure create configuration file include/ode/config.h
|
||||
make ode-lib build the core ODE library
|
||||
make drawstuff-lib build the OpenGL-based graphics library
|
||||
make ode-test build some ODE tests (they need drawstuff)
|
||||
make drawstuff-test build a test app for the drawstuff library
|
||||
|
||||
all of these targets will do an implicit 'make configure'. if the
|
||||
configurator screws up then you can edit the settings directly in
|
||||
include/ode/config.h.
|
||||
|
||||
(4) to install the ODE library onto your system you should copy the 'lib' and
|
||||
'include' directories to a suitable place, e.g. on unix:
|
||||
|
||||
include/ode --> /usr/local/include/ode
|
||||
lib/libode.a --> /usr/local/lib/libode.a
|
||||
|
||||
ODE has been verified to build on the following platforms:
|
||||
|
||||
config ode-lib ode-test
|
||||
------ ------- --------
|
||||
windows
|
||||
MSVC msvc * *
|
||||
MinGW mingw * *
|
||||
CygWin cygwin * *
|
||||
linux (x86, mandrake 8.1) unix-gcc * *
|
||||
linux (alpha, debian 2.2) unix-gcc * ?
|
||||
linux (RS/6000, debian 2.2) unix-gcc * ?
|
||||
linux (Sparc U60, debian 2.2) unix-gcc * ?
|
||||
freebsd 4.3 unix-gcc * ?
|
||||
Mac OS-X osx * ?
|
||||
Solaris 8 (Sparc R220) unix-gcc * ?
|
||||
34
extern/ode/dist/LICENSE-BSD.TXT
vendored
34
extern/ode/dist/LICENSE-BSD.TXT
vendored
@@ -1,34 +0,0 @@
|
||||
|
||||
This is the BSD-style license for the Open Dynamics Engine
|
||||
----------------------------------------------------------
|
||||
|
||||
Open Dynamics Engine
|
||||
Copyright (c) 2001,2002, Russell L. Smith.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions
|
||||
are met:
|
||||
|
||||
Redistributions of source code must retain the above copyright notice,
|
||||
this list of conditions and the following disclaimer.
|
||||
|
||||
Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
Neither the names of ODE's copyright owner nor the names of its
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
|
||||
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
|
||||
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
|
||||
TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
||||
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
|
||||
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
|
||||
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
502
extern/ode/dist/LICENSE.TXT
vendored
502
extern/ode/dist/LICENSE.TXT
vendored
@@ -1,502 +0,0 @@
|
||||
GNU LESSER GENERAL PUBLIC LICENSE
|
||||
Version 2.1, February 1999
|
||||
|
||||
Copyright (C) 1991, 1999 Free Software Foundation, Inc.
|
||||
59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
Everyone is permitted to copy and distribute verbatim copies
|
||||
of this license document, but changing it is not allowed.
|
||||
|
||||
[This is the first released version of the Lesser GPL. It also counts
|
||||
as the successor of the GNU Library Public License, version 2, hence
|
||||
the version number 2.1.]
|
||||
|
||||
Preamble
|
||||
|
||||
The licenses for most software are designed to take away your
|
||||
freedom to share and change it. By contrast, the GNU General Public
|
||||
Licenses are intended to guarantee your freedom to share and change
|
||||
free software--to make sure the software is free for all its users.
|
||||
|
||||
This license, the Lesser General Public License, applies to some
|
||||
specially designated software packages--typically libraries--of the
|
||||
Free Software Foundation and other authors who decide to use it. You
|
||||
can use it too, but we suggest you first think carefully about whether
|
||||
this license or the ordinary General Public License is the better
|
||||
strategy to use in any particular case, based on the explanations below.
|
||||
|
||||
When we speak of free software, we are referring to freedom of use,
|
||||
not price. Our General Public Licenses are designed to make sure that
|
||||
you have the freedom to distribute copies of free software (and charge
|
||||
for this service if you wish); that you receive source code or can get
|
||||
it if you want it; that you can change the software and use pieces of
|
||||
it in new free programs; and that you are informed that you can do
|
||||
these things.
|
||||
|
||||
To protect your rights, we need to make restrictions that forbid
|
||||
distributors to deny you these rights or to ask you to surrender these
|
||||
rights. These restrictions translate to certain responsibilities for
|
||||
you if you distribute copies of the library or if you modify it.
|
||||
|
||||
For example, if you distribute copies of the library, whether gratis
|
||||
or for a fee, you must give the recipients all the rights that we gave
|
||||
you. You must make sure that they, too, receive or can get the source
|
||||
code. If you link other code with the library, you must provide
|
||||
complete object files to the recipients, so that they can relink them
|
||||
with the library after making changes to the library and recompiling
|
||||
it. And you must show them these terms so they know their rights.
|
||||
|
||||
We protect your rights with a two-step method: (1) we copyright the
|
||||
library, and (2) we offer you this license, which gives you legal
|
||||
permission to copy, distribute and/or modify the library.
|
||||
|
||||
To protect each distributor, we want to make it very clear that
|
||||
there is no warranty for the free library. Also, if the library is
|
||||
modified by someone else and passed on, the recipients should know
|
||||
that what they have is not the original version, so that the original
|
||||
author's reputation will not be affected by problems that might be
|
||||
introduced by others.
|
||||
|
||||
Finally, software patents pose a constant threat to the existence of
|
||||
any free program. We wish to make sure that a company cannot
|
||||
effectively restrict the users of a free program by obtaining a
|
||||
restrictive license from a patent holder. Therefore, we insist that
|
||||
any patent license obtained for a version of the library must be
|
||||
consistent with the full freedom of use specified in this license.
|
||||
|
||||
Most GNU software, including some libraries, is covered by the
|
||||
ordinary GNU General Public License. This license, the GNU Lesser
|
||||
General Public License, applies to certain designated libraries, and
|
||||
is quite different from the ordinary General Public License. We use
|
||||
this license for certain libraries in order to permit linking those
|
||||
libraries into non-free programs.
|
||||
|
||||
When a program is linked with a library, whether statically or using
|
||||
a shared library, the combination of the two is legally speaking a
|
||||
combined work, a derivative of the original library. The ordinary
|
||||
General Public License therefore permits such linking only if the
|
||||
entire combination fits its criteria of freedom. The Lesser General
|
||||
Public License permits more lax criteria for linking other code with
|
||||
the library.
|
||||
|
||||
We call this license the "Lesser" General Public License because it
|
||||
does Less to protect the user's freedom than the ordinary General
|
||||
Public License. It also provides other free software developers Less
|
||||
of an advantage over competing non-free programs. These disadvantages
|
||||
are the reason we use the ordinary General Public License for many
|
||||
libraries. However, the Lesser license provides advantages in certain
|
||||
special circumstances.
|
||||
|
||||
For example, on rare occasions, there may be a special need to
|
||||
encourage the widest possible use of a certain library, so that it becomes
|
||||
a de-facto standard. To achieve this, non-free programs must be
|
||||
allowed to use the library. A more frequent case is that a free
|
||||
library does the same job as widely used non-free libraries. In this
|
||||
case, there is little to gain by limiting the free library to free
|
||||
software only, so we use the Lesser General Public License.
|
||||
|
||||
In other cases, permission to use a particular library in non-free
|
||||
programs enables a greater number of people to use a large body of
|
||||
free software. For example, permission to use the GNU C Library in
|
||||
non-free programs enables many more people to use the whole GNU
|
||||
operating system, as well as its variant, the GNU/Linux operating
|
||||
system.
|
||||
|
||||
Although the Lesser General Public License is Less protective of the
|
||||
users' freedom, it does ensure that the user of a program that is
|
||||
linked with the Library has the freedom and the wherewithal to run
|
||||
that program using a modified version of the Library.
|
||||
|
||||
The precise terms and conditions for copying, distribution and
|
||||
modification follow. Pay close attention to the difference between a
|
||||
"work based on the library" and a "work that uses the library". The
|
||||
former contains code derived from the library, whereas the latter must
|
||||
be combined with the library in order to run.
|
||||
|
||||
GNU LESSER GENERAL PUBLIC LICENSE
|
||||
TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
|
||||
|
||||
0. This License Agreement applies to any software library or other
|
||||
program which contains a notice placed by the copyright holder or
|
||||
other authorized party saying it may be distributed under the terms of
|
||||
this Lesser General Public License (also called "this License").
|
||||
Each licensee is addressed as "you".
|
||||
|
||||
A "library" means a collection of software functions and/or data
|
||||
prepared so as to be conveniently linked with application programs
|
||||
(which use some of those functions and data) to form executables.
|
||||
|
||||
The "Library", below, refers to any such software library or work
|
||||
which has been distributed under these terms. A "work based on the
|
||||
Library" means either the Library or any derivative work under
|
||||
copyright law: that is to say, a work containing the Library or a
|
||||
portion of it, either verbatim or with modifications and/or translated
|
||||
straightforwardly into another language. (Hereinafter, translation is
|
||||
included without limitation in the term "modification".)
|
||||
|
||||
"Source code" for a work means the preferred form of the work for
|
||||
making modifications to it. For a library, complete source code means
|
||||
all the source code for all modules it contains, plus any associated
|
||||
interface definition files, plus the scripts used to control compilation
|
||||
and installation of the library.
|
||||
|
||||
Activities other than copying, distribution and modification are not
|
||||
covered by this License; they are outside its scope. The act of
|
||||
running a program using the Library is not restricted, and output from
|
||||
such a program is covered only if its contents constitute a work based
|
||||
on the Library (independent of the use of the Library in a tool for
|
||||
writing it). Whether that is true depends on what the Library does
|
||||
and what the program that uses the Library does.
|
||||
|
||||
1. You may copy and distribute verbatim copies of the Library's
|
||||
complete source code as you receive it, in any medium, provided that
|
||||
you conspicuously and appropriately publish on each copy an
|
||||
appropriate copyright notice and disclaimer of warranty; keep intact
|
||||
all the notices that refer to this License and to the absence of any
|
||||
warranty; and distribute a copy of this License along with the
|
||||
Library.
|
||||
|
||||
You may charge a fee for the physical act of transferring a copy,
|
||||
and you may at your option offer warranty protection in exchange for a
|
||||
fee.
|
||||
|
||||
2. You may modify your copy or copies of the Library or any portion
|
||||
of it, thus forming a work based on the Library, and copy and
|
||||
distribute such modifications or work under the terms of Section 1
|
||||
above, provided that you also meet all of these conditions:
|
||||
|
||||
a) The modified work must itself be a software library.
|
||||
|
||||
b) You must cause the files modified to carry prominent notices
|
||||
stating that you changed the files and the date of any change.
|
||||
|
||||
c) You must cause the whole of the work to be licensed at no
|
||||
charge to all third parties under the terms of this License.
|
||||
|
||||
d) If a facility in the modified Library refers to a function or a
|
||||
table of data to be supplied by an application program that uses
|
||||
the facility, other than as an argument passed when the facility
|
||||
is invoked, then you must make a good faith effort to ensure that,
|
||||
in the event an application does not supply such function or
|
||||
table, the facility still operates, and performs whatever part of
|
||||
its purpose remains meaningful.
|
||||
|
||||
(For example, a function in a library to compute square roots has
|
||||
a purpose that is entirely well-defined independent of the
|
||||
application. Therefore, Subsection 2d requires that any
|
||||
application-supplied function or table used by this function must
|
||||
be optional: if the application does not supply it, the square
|
||||
root function must still compute square roots.)
|
||||
|
||||
These requirements apply to the modified work as a whole. If
|
||||
identifiable sections of that work are not derived from the Library,
|
||||
and can be reasonably considered independent and separate works in
|
||||
themselves, then this License, and its terms, do not apply to those
|
||||
sections when you distribute them as separate works. But when you
|
||||
distribute the same sections as part of a whole which is a work based
|
||||
on the Library, the distribution of the whole must be on the terms of
|
||||
this License, whose permissions for other licensees extend to the
|
||||
entire whole, and thus to each and every part regardless of who wrote
|
||||
it.
|
||||
|
||||
Thus, it is not the intent of this section to claim rights or contest
|
||||
your rights to work written entirely by you; rather, the intent is to
|
||||
exercise the right to control the distribution of derivative or
|
||||
collective works based on the Library.
|
||||
|
||||
In addition, mere aggregation of another work not based on the Library
|
||||
with the Library (or with a work based on the Library) on a volume of
|
||||
a storage or distribution medium does not bring the other work under
|
||||
the scope of this License.
|
||||
|
||||
3. You may opt to apply the terms of the ordinary GNU General Public
|
||||
License instead of this License to a given copy of the Library. To do
|
||||
this, you must alter all the notices that refer to this License, so
|
||||
that they refer to the ordinary GNU General Public License, version 2,
|
||||
instead of to this License. (If a newer version than version 2 of the
|
||||
ordinary GNU General Public License has appeared, then you can specify
|
||||
that version instead if you wish.) Do not make any other change in
|
||||
these notices.
|
||||
|
||||
Once this change is made in a given copy, it is irreversible for
|
||||
that copy, so the ordinary GNU General Public License applies to all
|
||||
subsequent copies and derivative works made from that copy.
|
||||
|
||||
This option is useful when you wish to copy part of the code of
|
||||
the Library into a program that is not a library.
|
||||
|
||||
4. You may copy and distribute the Library (or a portion or
|
||||
derivative of it, under Section 2) in object code or executable form
|
||||
under the terms of Sections 1 and 2 above provided that you accompany
|
||||
it with the complete corresponding machine-readable source code, which
|
||||
must be distributed under the terms of Sections 1 and 2 above on a
|
||||
medium customarily used for software interchange.
|
||||
|
||||
If distribution of object code is made by offering access to copy
|
||||
from a designated place, then offering equivalent access to copy the
|
||||
source code from the same place satisfies the requirement to
|
||||
distribute the source code, even though third parties are not
|
||||
compelled to copy the source along with the object code.
|
||||
|
||||
5. A program that contains no derivative of any portion of the
|
||||
Library, but is designed to work with the Library by being compiled or
|
||||
linked with it, is called a "work that uses the Library". Such a
|
||||
work, in isolation, is not a derivative work of the Library, and
|
||||
therefore falls outside the scope of this License.
|
||||
|
||||
However, linking a "work that uses the Library" with the Library
|
||||
creates an executable that is a derivative of the Library (because it
|
||||
contains portions of the Library), rather than a "work that uses the
|
||||
library". The executable is therefore covered by this License.
|
||||
Section 6 states terms for distribution of such executables.
|
||||
|
||||
When a "work that uses the Library" uses material from a header file
|
||||
that is part of the Library, the object code for the work may be a
|
||||
derivative work of the Library even though the source code is not.
|
||||
Whether this is true is especially significant if the work can be
|
||||
linked without the Library, or if the work is itself a library. The
|
||||
threshold for this to be true is not precisely defined by law.
|
||||
|
||||
If such an object file uses only numerical parameters, data
|
||||
structure layouts and accessors, and small macros and small inline
|
||||
functions (ten lines or less in length), then the use of the object
|
||||
file is unrestricted, regardless of whether it is legally a derivative
|
||||
work. (Executables containing this object code plus portions of the
|
||||
Library will still fall under Section 6.)
|
||||
|
||||
Otherwise, if the work is a derivative of the Library, you may
|
||||
distribute the object code for the work under the terms of Section 6.
|
||||
Any executables containing that work also fall under Section 6,
|
||||
whether or not they are linked directly with the Library itself.
|
||||
|
||||
6. As an exception to the Sections above, you may also combine or
|
||||
link a "work that uses the Library" with the Library to produce a
|
||||
work containing portions of the Library, and distribute that work
|
||||
under terms of your choice, provided that the terms permit
|
||||
modification of the work for the customer's own use and reverse
|
||||
engineering for debugging such modifications.
|
||||
|
||||
You must give prominent notice with each copy of the work that the
|
||||
Library is used in it and that the Library and its use are covered by
|
||||
this License. You must supply a copy of this License. If the work
|
||||
during execution displays copyright notices, you must include the
|
||||
copyright notice for the Library among them, as well as a reference
|
||||
directing the user to the copy of this License. Also, you must do one
|
||||
of these things:
|
||||
|
||||
a) Accompany the work with the complete corresponding
|
||||
machine-readable source code for the Library including whatever
|
||||
changes were used in the work (which must be distributed under
|
||||
Sections 1 and 2 above); and, if the work is an executable linked
|
||||
with the Library, with the complete machine-readable "work that
|
||||
uses the Library", as object code and/or source code, so that the
|
||||
user can modify the Library and then relink to produce a modified
|
||||
executable containing the modified Library. (It is understood
|
||||
that the user who changes the contents of definitions files in the
|
||||
Library will not necessarily be able to recompile the application
|
||||
to use the modified definitions.)
|
||||
|
||||
b) Use a suitable shared library mechanism for linking with the
|
||||
Library. A suitable mechanism is one that (1) uses at run time a
|
||||
copy of the library already present on the user's computer system,
|
||||
rather than copying library functions into the executable, and (2)
|
||||
will operate properly with a modified version of the library, if
|
||||
the user installs one, as long as the modified version is
|
||||
interface-compatible with the version that the work was made with.
|
||||
|
||||
c) Accompany the work with a written offer, valid for at
|
||||
least three years, to give the same user the materials
|
||||
specified in Subsection 6a, above, for a charge no more
|
||||
than the cost of performing this distribution.
|
||||
|
||||
d) If distribution of the work is made by offering access to copy
|
||||
from a designated place, offer equivalent access to copy the above
|
||||
specified materials from the same place.
|
||||
|
||||
e) Verify that the user has already received a copy of these
|
||||
materials or that you have already sent this user a copy.
|
||||
|
||||
For an executable, the required form of the "work that uses the
|
||||
Library" must include any data and utility programs needed for
|
||||
reproducing the executable from it. However, as a special exception,
|
||||
the materials to be distributed need not include anything that is
|
||||
normally distributed (in either source or binary form) with the major
|
||||
components (compiler, kernel, and so on) of the operating system on
|
||||
which the executable runs, unless that component itself accompanies
|
||||
the executable.
|
||||
|
||||
It may happen that this requirement contradicts the license
|
||||
restrictions of other proprietary libraries that do not normally
|
||||
accompany the operating system. Such a contradiction means you cannot
|
||||
use both them and the Library together in an executable that you
|
||||
distribute.
|
||||
|
||||
7. You may place library facilities that are a work based on the
|
||||
Library side-by-side in a single library together with other library
|
||||
facilities not covered by this License, and distribute such a combined
|
||||
library, provided that the separate distribution of the work based on
|
||||
the Library and of the other library facilities is otherwise
|
||||
permitted, and provided that you do these two things:
|
||||
|
||||
a) Accompany the combined library with a copy of the same work
|
||||
based on the Library, uncombined with any other library
|
||||
facilities. This must be distributed under the terms of the
|
||||
Sections above.
|
||||
|
||||
b) Give prominent notice with the combined library of the fact
|
||||
that part of it is a work based on the Library, and explaining
|
||||
where to find the accompanying uncombined form of the same work.
|
||||
|
||||
8. You may not copy, modify, sublicense, link with, or distribute
|
||||
the Library except as expressly provided under this License. Any
|
||||
attempt otherwise to copy, modify, sublicense, link with, or
|
||||
distribute the Library is void, and will automatically terminate your
|
||||
rights under this License. However, parties who have received copies,
|
||||
or rights, from you under this License will not have their licenses
|
||||
terminated so long as such parties remain in full compliance.
|
||||
|
||||
9. You are not required to accept this License, since you have not
|
||||
signed it. However, nothing else grants you permission to modify or
|
||||
distribute the Library or its derivative works. These actions are
|
||||
prohibited by law if you do not accept this License. Therefore, by
|
||||
modifying or distributing the Library (or any work based on the
|
||||
Library), you indicate your acceptance of this License to do so, and
|
||||
all its terms and conditions for copying, distributing or modifying
|
||||
the Library or works based on it.
|
||||
|
||||
10. Each time you redistribute the Library (or any work based on the
|
||||
Library), the recipient automatically receives a license from the
|
||||
original licensor to copy, distribute, link with or modify the Library
|
||||
subject to these terms and conditions. You may not impose any further
|
||||
restrictions on the recipients' exercise of the rights granted herein.
|
||||
You are not responsible for enforcing compliance by third parties with
|
||||
this License.
|
||||
|
||||
11. If, as a consequence of a court judgment or allegation of patent
|
||||
infringement or for any other reason (not limited to patent issues),
|
||||
conditions are imposed on you (whether by court order, agreement or
|
||||
otherwise) that contradict the conditions of this License, they do not
|
||||
excuse you from the conditions of this License. If you cannot
|
||||
distribute so as to satisfy simultaneously your obligations under this
|
||||
License and any other pertinent obligations, then as a consequence you
|
||||
may not distribute the Library at all. For example, if a patent
|
||||
license would not permit royalty-free redistribution of the Library by
|
||||
all those who receive copies directly or indirectly through you, then
|
||||
the only way you could satisfy both it and this License would be to
|
||||
refrain entirely from distribution of the Library.
|
||||
|
||||
If any portion of this section is held invalid or unenforceable under any
|
||||
particular circumstance, the balance of the section is intended to apply,
|
||||
and the section as a whole is intended to apply in other circumstances.
|
||||
|
||||
It is not the purpose of this section to induce you to infringe any
|
||||
patents or other property right claims or to contest validity of any
|
||||
such claims; this section has the sole purpose of protecting the
|
||||
integrity of the free software distribution system which is
|
||||
implemented by public license practices. Many people have made
|
||||
generous contributions to the wide range of software distributed
|
||||
through that system in reliance on consistent application of that
|
||||
system; it is up to the author/donor to decide if he or she is willing
|
||||
to distribute software through any other system and a licensee cannot
|
||||
impose that choice.
|
||||
|
||||
This section is intended to make thoroughly clear what is believed to
|
||||
be a consequence of the rest of this License.
|
||||
|
||||
12. If the distribution and/or use of the Library is restricted in
|
||||
certain countries either by patents or by copyrighted interfaces, the
|
||||
original copyright holder who places the Library under this License may add
|
||||
an explicit geographical distribution limitation excluding those countries,
|
||||
so that distribution is permitted only in or among countries not thus
|
||||
excluded. In such case, this License incorporates the limitation as if
|
||||
written in the body of this License.
|
||||
|
||||
13. The Free Software Foundation may publish revised and/or new
|
||||
versions of the Lesser General Public License from time to time.
|
||||
Such new versions will be similar in spirit to the present version,
|
||||
but may differ in detail to address new problems or concerns.
|
||||
|
||||
Each version is given a distinguishing version number. If the Library
|
||||
specifies a version number of this License which applies to it and
|
||||
"any later version", you have the option of following the terms and
|
||||
conditions either of that version or of any later version published by
|
||||
the Free Software Foundation. If the Library does not specify a
|
||||
license version number, you may choose any version ever published by
|
||||
the Free Software Foundation.
|
||||
|
||||
14. If you wish to incorporate parts of the Library into other free
|
||||
programs whose distribution conditions are incompatible with these,
|
||||
write to the author to ask for permission. For software which is
|
||||
copyrighted by the Free Software Foundation, write to the Free
|
||||
Software Foundation; we sometimes make exceptions for this. Our
|
||||
decision will be guided by the two goals of preserving the free status
|
||||
of all derivatives of our free software and of promoting the sharing
|
||||
and reuse of software generally.
|
||||
|
||||
NO WARRANTY
|
||||
|
||||
15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
|
||||
WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
|
||||
EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
|
||||
OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
|
||||
KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
|
||||
LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
|
||||
THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
|
||||
|
||||
16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
|
||||
WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
|
||||
AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
|
||||
FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
|
||||
CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
|
||||
LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
|
||||
RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
|
||||
FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
|
||||
SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
|
||||
DAMAGES.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
How to Apply These Terms to Your New Libraries
|
||||
|
||||
If you develop a new library, and you want it to be of the greatest
|
||||
possible use to the public, we recommend making it free software that
|
||||
everyone can redistribute and change. You can do so by permitting
|
||||
redistribution under these terms (or, alternatively, under the terms of the
|
||||
ordinary General Public License).
|
||||
|
||||
To apply these terms, attach the following notices to the library. It is
|
||||
safest to attach them to the start of each source file to most effectively
|
||||
convey the exclusion of warranty; and each file should have at least the
|
||||
"copyright" line and a pointer to where the full notice is found.
|
||||
|
||||
<one line to give the library's name and a brief idea of what it does.>
|
||||
Copyright (C) <year> <name of author>
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
This library is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
Lesser General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU Lesser General Public
|
||||
License along with this library; if not, write to the Free Software
|
||||
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
|
||||
Also add information on how to contact you by electronic and paper mail.
|
||||
|
||||
You should also get your employer (if you work as a programmer) or your
|
||||
school, if any, to sign a "copyright disclaimer" for the library, if
|
||||
necessary. Here is a sample; alter the names:
|
||||
|
||||
Yoyodyne, Inc., hereby disclaims all copyright interest in the
|
||||
library `Frob' (a library for tweaking knobs) written by James Random Hacker.
|
||||
|
||||
<signature of Ty Coon>, 1 April 1990
|
||||
Ty Coon, President of Vice
|
||||
|
||||
That's all there is to it!
|
||||
280
extern/ode/dist/Makefile
vendored
280
extern/ode/dist/Makefile
vendored
@@ -1,280 +0,0 @@
|
||||
#########################################################################
|
||||
# #
|
||||
# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. #
|
||||
# All rights reserved. Email: russ@q12.org Web: www.q12.org #
|
||||
# #
|
||||
# This library is free software; you can redistribute it and/or #
|
||||
# modify it under the terms of EITHER: #
|
||||
# (1) The GNU Lesser General Public License as published by the Free #
|
||||
# Software Foundation; either version 2.1 of the License, or (at #
|
||||
# your option) any later version. The text of the GNU Lesser #
|
||||
# General Public License is included with this library in the #
|
||||
# file LICENSE.TXT. #
|
||||
# (2) The BSD-style license that is included with this library in #
|
||||
# the file LICENSE-BSD.TXT. #
|
||||
# #
|
||||
# This library is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files #
|
||||
# LICENSE.TXT and LICENSE-BSD.TXT for more details. #
|
||||
# #
|
||||
#########################################################################
|
||||
|
||||
USER_SETTINGS=config/user-settings
|
||||
include $(USER_SETTINGS)
|
||||
PLATFORM_MAKEFILE=config/makefile.$(PLATFORM)
|
||||
include $(PLATFORM_MAKEFILE)
|
||||
|
||||
##############################################################################
|
||||
# check some variables that were supposed to be defined
|
||||
|
||||
ifneq ($(BUILD),debug)
|
||||
ifneq ($(BUILD),release)
|
||||
$(error the BUILD variable is not set properly)
|
||||
endif
|
||||
endif
|
||||
|
||||
ifneq ($(PRECISION),SINGLE)
|
||||
ifneq ($(PRECISION),DOUBLE)
|
||||
$(error the PRECISION variable is not set properly)
|
||||
endif
|
||||
endif
|
||||
|
||||
##############################################################################
|
||||
# package settings
|
||||
|
||||
ODE_SRC = \
|
||||
ode/src/array.cpp \
|
||||
ode/src/error.cpp \
|
||||
ode/src/memory.cpp \
|
||||
ode/src/obstack.cpp \
|
||||
ode/src/odemath.cpp \
|
||||
ode/src/matrix.cpp \
|
||||
ode/src/misc.cpp \
|
||||
ode/src/rotation.cpp \
|
||||
ode/src/mass.cpp \
|
||||
ode/src/ode.cpp \
|
||||
ode/src/step.cpp \
|
||||
ode/src/lcp.cpp \
|
||||
ode/src/joint.cpp \
|
||||
ode/src/space.cpp \
|
||||
ode/src/geom.cpp \
|
||||
ode/src/timer.cpp \
|
||||
ode/src/mat.cpp \
|
||||
ode/src/testing.cpp
|
||||
ODE_PREGEN_SRC = \
|
||||
ode/src/fastldlt.c \
|
||||
ode/src/fastlsolve.c \
|
||||
ode/src/fastltsolve.c \
|
||||
ode/src/fastdot.c
|
||||
|
||||
ifeq ($(WINDOWS),1)
|
||||
DRAWSTUFF_SRC = drawstuff/src/drawstuff.cpp drawstuff/src/windows.cpp
|
||||
RESOURCE_FILE=lib/resources.RES
|
||||
else
|
||||
DRAWSTUFF_SRC = drawstuff/src/drawstuff.cpp drawstuff/src/x11.cpp
|
||||
endif
|
||||
|
||||
ODE_LIB_NAME=ode
|
||||
DRAWSTUFF_LIB_NAME=drawstuff
|
||||
|
||||
INCPATH=include
|
||||
LIBPATH=lib
|
||||
|
||||
ODE_TEST_SRC_CPP = \
|
||||
ode/test/test_ode.cpp \
|
||||
ode/test/test_chain2.cpp \
|
||||
ode/test/test_hinge.cpp \
|
||||
ode/test/test_slider.cpp \
|
||||
ode/test/test_collision.cpp \
|
||||
ode/test/test_boxstack.cpp \
|
||||
ode/test/test_buggy.cpp \
|
||||
ode/test/test_joints.cpp \
|
||||
ode/test/test_space.cpp \
|
||||
ode/test/test_I.cpp \
|
||||
ode/test/test_step.cpp \
|
||||
ode/test/test_friction.cpp
|
||||
ODE_TEST_SRC_C = \
|
||||
ode/test/test_chain1.c
|
||||
DRAWSTUFF_TEST_SRC_CPP = \
|
||||
drawstuff/dstest/dstest.cpp
|
||||
|
||||
CONFIGURATOR_SRC=configurator.c
|
||||
CONFIG_H=include/ode/config.h
|
||||
|
||||
##############################################################################
|
||||
# derived things
|
||||
|
||||
DEFINES=
|
||||
|
||||
# add some defines depending on the build mode
|
||||
ifeq ($(BUILD),release)
|
||||
DEFINES+=$(C_DEF)dNODEBUG
|
||||
endif
|
||||
ifeq ($(BUILD),debug)
|
||||
DEFINES+=$(C_DEF)dDEBUG_ALLOC
|
||||
endif
|
||||
|
||||
# object file names
|
||||
ODE_PREGEN_OBJECTS=$(ODE_PREGEN_SRC:%.c=%$(OBJ))
|
||||
ODE_OBJECTS=$(ODE_SRC:%.cpp=%$(OBJ)) $(ODE_PREGEN_OBJECTS)
|
||||
DRAWSTUFF_OBJECTS=$(DRAWSTUFF_SRC:%.cpp=%$(OBJ)) $(RESOURCE_FILE)
|
||||
|
||||
# side-effect variables causing creation of files containing lists of
|
||||
# filenames to be linked, to work around command-line-length limitations
|
||||
# on outdated 16-bit operating systems. because of command-line length
|
||||
# limitations we cannot issue a link command with all object filenames
|
||||
# specified (because this command is too long and overflows the command
|
||||
# buffer), but instead must create a file containing all object filenames
|
||||
# to be linked, and specify this list-file with @listfile on the command-line.
|
||||
#
|
||||
# the difficult part is doing this in a flexible way; we don't want to
|
||||
# hard-code the to-be-linked object filenames in a file, but instead
|
||||
# want to dynamically create a file containing a list of all object filenames
|
||||
# within the $XXX_OBJECTS makefile variables. to do this, we use side-effect
|
||||
# variables.
|
||||
#
|
||||
# idea: when these variables are EVALUATED (i.e. later during rule execution,
|
||||
# not now during variable definition), they cause a SIDE EFFECT which creates
|
||||
# a file with the list of all ODE object files. why the chicanery??? because
|
||||
# if we have a command-line length limitation, no SINGLE command we issue will
|
||||
# be able to create a file containing all object files to be linked
|
||||
# (because that command itself would need to include all filenames, making
|
||||
# it too long to be executed). instead, we must use the gnu-make "foreach"
|
||||
# function, combined - probably in an unintended way - with the "shell"
|
||||
# function. this is probably unintended because we are not using the "shell"
|
||||
# function to return a string value for variable evaluation, but are instead
|
||||
# using the "shell" function to cause a side effect (appending of each filename
|
||||
# to the filename-list-file).
|
||||
#
|
||||
# one possible snag is that, forbidding use of any external EXE utilities and
|
||||
# relying only on the facilities provided by the outdated 16-bit operating
|
||||
# system, there is no way to issue a SERIES of commands which append text to
|
||||
# the end of a file WITHOUT adding newlines. therefore, the list of to-be-
|
||||
# linked object files is separated by newlines in the list file. fortunately,
|
||||
# the linker utility for this outdated 16-bit operating system accepts
|
||||
# filenames on separate lines in the list file.
|
||||
|
||||
# remember: when we evaluate these variables later, this causes the creation
|
||||
# of the appropriate list file.
|
||||
ifeq ($(WINDOWS16),1)
|
||||
SIDE_EFFECT_ODE_OBJLIST = $(foreach o,$(ODE_OBJECTS),$(shell echo $(o) >> odeobj.txt ))
|
||||
SIDE_EFFECT_DRAWSTUFF_OBJLIST = $(foreach o,$(DRAWSTUFF_OBJECTS),$(shell echo $(o) >> dsobj.txt ))
|
||||
endif
|
||||
|
||||
# library file names
|
||||
ODE_LIB=$(LIBPATH)/$(LIB_PREFIX)$(ODE_LIB_NAME)$(LIB_SUFFIX)
|
||||
DRAWSTUFF_LIB=$(LIBPATH)/$(LIB_PREFIX)$(DRAWSTUFF_LIB_NAME)$(LIB_SUFFIX)
|
||||
|
||||
# executable file names
|
||||
ODE_TEST_EXE=$(ODE_TEST_SRC_CPP:%.cpp=%.exe) $(ODE_TEST_SRC_C:%.c=%.exe)
|
||||
DRAWSTUFF_TEST_EXE=$(DRAWSTUFF_TEST_SRC_CPP:%.cpp=%.exe)
|
||||
CONFIGURATOR_EXE=$(CONFIGURATOR_SRC:%.c=%.exe)
|
||||
|
||||
##############################################################################
|
||||
# rules
|
||||
#
|
||||
# NOTE: the '.c' files are pregenerated sources, and must be compiled with
|
||||
# -O1 optimization. that is why the rule for .c files is a bit different.
|
||||
# why should it be compiled with O1? it is numerical code that is generated
|
||||
# by fbuild. O1 optimization is used to preserve the operation orders that
|
||||
# were discovered by fbuild to be the fastest on that platform. believe it or
|
||||
# not, O2 makes this code run much slower for most compilers.
|
||||
|
||||
debug: all
|
||||
all: ode-lib drawstuff-lib ode-test drawstuff-test
|
||||
@echo SUCCESS
|
||||
|
||||
ode-lib: configure $(ODE_LIB)
|
||||
drawstuff-lib: configure $(DRAWSTUFF_LIB)
|
||||
ode-test: ode-lib drawstuff-lib $(ODE_TEST_EXE)
|
||||
drawstuff-test: drawstuff-lib $(DRAWSTUFF_TEST_EXE)
|
||||
|
||||
ifndef ODE_LIB_AR_RULE
|
||||
ODE_LIB_AR_RULE=$(AR)$@
|
||||
endif
|
||||
|
||||
$(ODE_LIB): pre_ode_lib $(ODE_OBJECTS)
|
||||
ifeq ($(WINDOWS16),1)
|
||||
# if we have a command-line-length limitation, then dynamically create
|
||||
# a file containing all object filenames, and pass this file to the linker
|
||||
# instead of directly specifying the object filenames on the command line.
|
||||
# the very evaluation of the following variable causes creation of file
|
||||
# odeobj.txt
|
||||
$(SIDE_EFFECT_ODE_OBJLIST)
|
||||
$(ODE_LIB_AR_RULE) @odeobj.txt
|
||||
else
|
||||
# if we have no command-line-length limitation, directly specify all
|
||||
# object files to be linked.
|
||||
$(ODE_LIB_AR_RULE) $(ODE_OBJECTS)
|
||||
endif
|
||||
|
||||
ifdef RANLIB
|
||||
$(RANLIB) $@
|
||||
endif
|
||||
|
||||
$(DRAWSTUFF_LIB): pre_drawstuff_lib $(DRAWSTUFF_OBJECTS)
|
||||
ifeq ($WINDOWS16),1)
|
||||
# if we have a command-line-length limitation, then do the same as above.
|
||||
$(SIDE_EFFECT_DRAWSTUFF_OBJLIST)
|
||||
$(AR)$@ @dsobj.txt
|
||||
else
|
||||
# if we have no command-line-length limitation, directly specify all object
|
||||
# files to be linked.
|
||||
$(AR)$@ $(DRAWSTUFF_OBJECTS)
|
||||
endif
|
||||
ifdef RANLIB
|
||||
$(RANLIB) $@
|
||||
endif
|
||||
|
||||
# rules to be executed before library linking starts: delete list file (if one is used)
|
||||
|
||||
pre_ode_lib:
|
||||
ifeq ($WINDOWS16),1)
|
||||
$(DEL_CMD) odeobj.txt
|
||||
endif
|
||||
|
||||
pre_drawstuff_lib:
|
||||
ifeq ($WINDOWS16),1)
|
||||
$(DEL_CMD) dsobj.txt
|
||||
endif
|
||||
|
||||
clean:
|
||||
-$(DEL_CMD) $(ODE_OBJECTS) $(ODE_TEST_EXE) $(ODE_LIB) $(DRAWSTUFF_OBJECTS) $(DRAWSTUFF_TEST_EXE) $(DRAWSTUFF_LIB) ode/test/*$(OBJ) drawstuff/dstest/*$(OBJ) $(CONFIGURATOR_EXE) $(CONFIG_H)
|
||||
|
||||
%$(OBJ): %.c
|
||||
$(CC) $(C_FLAGS) $(C_INC)$(INCPATH) $(DEFINES) $(C_OPT)1 $(C_OUT)$@ $<
|
||||
|
||||
%$(OBJ): %.cpp
|
||||
$(CC) $(C_FLAGS) $(C_INC)$(INCPATH) $(DEFINES) $(C_OPT)$(OPT) $(C_OUT)$@ $<
|
||||
|
||||
%.exe: %$(OBJ)
|
||||
$(CC) $(C_EXEOUT)$@ $< $(ODE_LIB) $(DRAWSTUFF_LIB) $(RESOURCE_FILE) $(LINK_OPENGL) $(LINK_MATH)
|
||||
|
||||
# windows specific rules
|
||||
|
||||
lib/resources.RES: drawstuff/src/resources.rc
|
||||
$(RC_RULE)
|
||||
|
||||
|
||||
# configurator rules
|
||||
|
||||
configure: $(CONFIG_H)
|
||||
|
||||
$(CONFIG_H): $(CONFIGURATOR_EXE) $(USER_SETTINGS) $(PLATFORM_MAKEFILE)
|
||||
$(THIS_DIR)$(CONFIGURATOR_EXE) $(CONFIG_H) "$(CC) $(DEFINES) $(C_EXEOUT)" "$(DEL_CMD)" $(THIS_DIR)
|
||||
|
||||
$(CONFIGURATOR_EXE): $(CONFIGURATOR_SRC) $(USER_SETTINGS) $(PLATFORM_MAKEFILE)
|
||||
$(CC) $(C_DEF)d$(PRECISION) $(DEFINES) $(C_EXEOUT)$@ $<
|
||||
|
||||
|
||||
# unix-gcc specific dependency making
|
||||
|
||||
DEP_RULE=gcc -M $(C_INC)$(INCPATH) $(DEFINES)
|
||||
depend: $(ODE_SRC) $(ODE_PREGEN_SRC) $(DRAWSTUFF_SRC) $(ODE_TEST_SRC_CPP) $(ODE_TEST_SRC_C) $(DRAWSTUFF_TEST_SRC_CPP)
|
||||
$(DEP_RULE) $(ODE_SRC) $(ODE_PREGEN_SRC) | tools/process_deps ode/src/ > Makefile.deps
|
||||
$(DEP_RULE) $(DRAWSTUFF_SRC) | tools/process_deps drawstuff/src/ >> Makefile.deps
|
||||
$(DEP_RULE) $(ODE_TEST_SRC_CPP) | tools/process_deps ode/test/ >> Makefile.deps
|
||||
$(DEP_RULE) $(DRAWSTUFF_TEST_SRC_CPP) | tools/process_deps drawstuff/dstest/ >> Makefile.deps
|
||||
|
||||
include Makefile.deps
|
||||
456
extern/ode/dist/Makefile.deps
vendored
456
extern/ode/dist/Makefile.deps
vendored
@@ -1,456 +0,0 @@
|
||||
ode/src/array.o: \
|
||||
ode/src/array.cpp \
|
||||
include/ode/config.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/error.h \
|
||||
ode/src/array.h
|
||||
ode/src/error.o: \
|
||||
ode/src/error.cpp \
|
||||
include/ode/config.h \
|
||||
include/ode/error.h
|
||||
ode/src/memory.o: \
|
||||
ode/src/memory.cpp \
|
||||
include/ode/config.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/error.h
|
||||
ode/src/obstack.o: \
|
||||
ode/src/obstack.cpp \
|
||||
include/ode/common.h \
|
||||
include/ode/config.h \
|
||||
include/ode/error.h \
|
||||
include/ode/memory.h \
|
||||
ode/src/obstack.h \
|
||||
ode/src/objects.h \
|
||||
include/ode/mass.h \
|
||||
ode/src/array.h
|
||||
ode/src/odemath.o: \
|
||||
ode/src/odemath.cpp \
|
||||
include/ode/common.h \
|
||||
include/ode/config.h \
|
||||
include/ode/error.h \
|
||||
include/ode/odemath.h
|
||||
ode/src/matrix.o: \
|
||||
ode/src/matrix.cpp \
|
||||
include/ode/common.h \
|
||||
include/ode/config.h \
|
||||
include/ode/error.h \
|
||||
include/ode/matrix.h
|
||||
ode/src/misc.o: \
|
||||
ode/src/misc.cpp \
|
||||
include/ode/config.h \
|
||||
include/ode/misc.h \
|
||||
include/ode/common.h \
|
||||
include/ode/error.h \
|
||||
include/ode/matrix.h
|
||||
ode/src/rotation.o: \
|
||||
ode/src/rotation.cpp \
|
||||
include/ode/rotation.h \
|
||||
include/ode/common.h \
|
||||
include/ode/config.h \
|
||||
include/ode/error.h
|
||||
ode/src/mass.o: \
|
||||
ode/src/mass.cpp \
|
||||
include/ode/config.h \
|
||||
include/ode/mass.h \
|
||||
include/ode/common.h \
|
||||
include/ode/error.h \
|
||||
include/ode/odemath.h \
|
||||
include/ode/matrix.h
|
||||
ode/src/ode.o: \
|
||||
ode/src/ode.cpp \
|
||||
ode/src/objects.h \
|
||||
include/ode/common.h \
|
||||
include/ode/config.h \
|
||||
include/ode/error.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/mass.h \
|
||||
ode/src/array.h \
|
||||
include/ode/ode.h \
|
||||
include/ode/contact.h \
|
||||
include/ode/odemath.h \
|
||||
include/ode/matrix.h \
|
||||
include/ode/timer.h \
|
||||
include/ode/rotation.h \
|
||||
include/ode/space.h \
|
||||
include/ode/geom.h \
|
||||
include/ode/misc.h \
|
||||
include/ode/objects.h \
|
||||
include/ode/odecpp.h \
|
||||
ode/src/joint.h \
|
||||
ode/src/obstack.h \
|
||||
ode/src/step.h
|
||||
ode/src/step.o: \
|
||||
ode/src/step.cpp \
|
||||
ode/src/objects.h \
|
||||
include/ode/common.h \
|
||||
include/ode/config.h \
|
||||
include/ode/error.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/mass.h \
|
||||
ode/src/array.h \
|
||||
ode/src/joint.h \
|
||||
include/ode/contact.h \
|
||||
ode/src/obstack.h \
|
||||
include/ode/odemath.h \
|
||||
include/ode/rotation.h \
|
||||
include/ode/timer.h \
|
||||
include/ode/matrix.h \
|
||||
ode/src/lcp.h
|
||||
ode/src/lcp.o: \
|
||||
ode/src/lcp.cpp \
|
||||
include/ode/common.h \
|
||||
include/ode/config.h \
|
||||
include/ode/error.h \
|
||||
ode/src/lcp.h \
|
||||
include/ode/matrix.h \
|
||||
include/ode/misc.h \
|
||||
ode/src/mat.h \
|
||||
include/ode/timer.h
|
||||
ode/src/joint.o: \
|
||||
ode/src/joint.cpp \
|
||||
include/ode/odemath.h \
|
||||
include/ode/common.h \
|
||||
include/ode/config.h \
|
||||
include/ode/error.h \
|
||||
include/ode/rotation.h \
|
||||
include/ode/matrix.h \
|
||||
ode/src/joint.h \
|
||||
ode/src/objects.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/mass.h \
|
||||
ode/src/array.h \
|
||||
include/ode/contact.h \
|
||||
ode/src/obstack.h
|
||||
ode/src/space.o: \
|
||||
ode/src/space.cpp \
|
||||
include/ode/common.h \
|
||||
include/ode/config.h \
|
||||
include/ode/error.h \
|
||||
include/ode/space.h \
|
||||
include/ode/geom.h \
|
||||
include/ode/contact.h \
|
||||
include/ode/memory.h \
|
||||
ode/src/objects.h \
|
||||
include/ode/mass.h \
|
||||
ode/src/array.h \
|
||||
ode/src/geom_internal.h
|
||||
ode/src/geom.o: \
|
||||
ode/src/geom.cpp \
|
||||
include/ode/common.h \
|
||||
include/ode/config.h \
|
||||
include/ode/error.h \
|
||||
include/ode/geom.h \
|
||||
include/ode/space.h \
|
||||
include/ode/contact.h \
|
||||
include/ode/rotation.h \
|
||||
include/ode/odemath.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/misc.h \
|
||||
include/ode/objects.h \
|
||||
include/ode/mass.h \
|
||||
include/ode/matrix.h \
|
||||
ode/src/objects.h \
|
||||
ode/src/array.h \
|
||||
ode/src/geom_internal.h
|
||||
ode/src/timer.o: \
|
||||
ode/src/timer.cpp \
|
||||
include/ode/common.h \
|
||||
include/ode/config.h \
|
||||
include/ode/error.h \
|
||||
include/ode/timer.h
|
||||
ode/src/mat.o: \
|
||||
ode/src/mat.cpp \
|
||||
include/ode/config.h \
|
||||
include/ode/misc.h \
|
||||
include/ode/common.h \
|
||||
include/ode/error.h \
|
||||
include/ode/matrix.h \
|
||||
include/ode/memory.h \
|
||||
ode/src/mat.h
|
||||
ode/src/testing.o: \
|
||||
ode/src/testing.cpp \
|
||||
include/ode/config.h \
|
||||
include/ode/misc.h \
|
||||
include/ode/common.h \
|
||||
include/ode/error.h \
|
||||
include/ode/memory.h \
|
||||
ode/src/testing.h \
|
||||
ode/src/array.h
|
||||
ode/src/fastldlt.o: \
|
||||
ode/src/fastldlt.c \
|
||||
include/ode/matrix.h \
|
||||
include/ode/common.h \
|
||||
include/ode/config.h \
|
||||
include/ode/error.h
|
||||
ode/src/fastlsolve.o: \
|
||||
ode/src/fastlsolve.c \
|
||||
include/ode/matrix.h \
|
||||
include/ode/common.h \
|
||||
include/ode/config.h \
|
||||
include/ode/error.h
|
||||
ode/src/fastltsolve.o: \
|
||||
ode/src/fastltsolve.c \
|
||||
include/ode/matrix.h \
|
||||
include/ode/common.h \
|
||||
include/ode/config.h \
|
||||
include/ode/error.h
|
||||
ode/src/fastdot.o: \
|
||||
ode/src/fastdot.c \
|
||||
include/ode/matrix.h \
|
||||
include/ode/common.h \
|
||||
include/ode/config.h \
|
||||
include/ode/error.h
|
||||
drawstuff/src/drawstuff.o: \
|
||||
drawstuff/src/drawstuff.cpp \
|
||||
include/ode/config.h \
|
||||
include/drawstuff/drawstuff.h \
|
||||
include/drawstuff/version.h \
|
||||
drawstuff/src/internal.h
|
||||
drawstuff/src/x11.o: \
|
||||
drawstuff/src/x11.cpp \
|
||||
include/ode/config.h \
|
||||
include/drawstuff/drawstuff.h \
|
||||
include/drawstuff/version.h \
|
||||
drawstuff/src/internal.h
|
||||
ode/test/test_ode.o: \
|
||||
ode/test/test_ode.cpp \
|
||||
include/ode/ode.h \
|
||||
include/ode/config.h \
|
||||
include/ode/contact.h \
|
||||
include/ode/common.h \
|
||||
include/ode/error.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/odemath.h \
|
||||
include/ode/matrix.h \
|
||||
include/ode/timer.h \
|
||||
include/ode/rotation.h \
|
||||
include/ode/mass.h \
|
||||
include/ode/space.h \
|
||||
include/ode/geom.h \
|
||||
include/ode/misc.h \
|
||||
include/ode/objects.h \
|
||||
include/ode/odecpp.h
|
||||
ode/test/test_chain2.o: \
|
||||
ode/test/test_chain2.cpp \
|
||||
include/ode/ode.h \
|
||||
include/ode/config.h \
|
||||
include/ode/contact.h \
|
||||
include/ode/common.h \
|
||||
include/ode/error.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/odemath.h \
|
||||
include/ode/matrix.h \
|
||||
include/ode/timer.h \
|
||||
include/ode/rotation.h \
|
||||
include/ode/mass.h \
|
||||
include/ode/space.h \
|
||||
include/ode/geom.h \
|
||||
include/ode/misc.h \
|
||||
include/ode/objects.h \
|
||||
include/ode/odecpp.h \
|
||||
include/drawstuff/drawstuff.h \
|
||||
include/drawstuff/version.h
|
||||
ode/test/test_hinge.o: \
|
||||
ode/test/test_hinge.cpp \
|
||||
include/ode/ode.h \
|
||||
include/ode/config.h \
|
||||
include/ode/contact.h \
|
||||
include/ode/common.h \
|
||||
include/ode/error.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/odemath.h \
|
||||
include/ode/matrix.h \
|
||||
include/ode/timer.h \
|
||||
include/ode/rotation.h \
|
||||
include/ode/mass.h \
|
||||
include/ode/space.h \
|
||||
include/ode/geom.h \
|
||||
include/ode/misc.h \
|
||||
include/ode/objects.h \
|
||||
include/ode/odecpp.h \
|
||||
include/drawstuff/drawstuff.h \
|
||||
include/drawstuff/version.h
|
||||
ode/test/test_slider.o: \
|
||||
ode/test/test_slider.cpp \
|
||||
include/ode/ode.h \
|
||||
include/ode/config.h \
|
||||
include/ode/contact.h \
|
||||
include/ode/common.h \
|
||||
include/ode/error.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/odemath.h \
|
||||
include/ode/matrix.h \
|
||||
include/ode/timer.h \
|
||||
include/ode/rotation.h \
|
||||
include/ode/mass.h \
|
||||
include/ode/space.h \
|
||||
include/ode/geom.h \
|
||||
include/ode/misc.h \
|
||||
include/ode/objects.h \
|
||||
include/ode/odecpp.h \
|
||||
include/drawstuff/drawstuff.h \
|
||||
include/drawstuff/version.h
|
||||
ode/test/test_collision.o: \
|
||||
ode/test/test_collision.cpp \
|
||||
include/ode/ode.h \
|
||||
include/ode/config.h \
|
||||
include/ode/contact.h \
|
||||
include/ode/common.h \
|
||||
include/ode/error.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/odemath.h \
|
||||
include/ode/matrix.h \
|
||||
include/ode/timer.h \
|
||||
include/ode/rotation.h \
|
||||
include/ode/mass.h \
|
||||
include/ode/space.h \
|
||||
include/ode/geom.h \
|
||||
include/ode/misc.h \
|
||||
include/ode/objects.h \
|
||||
include/ode/odecpp.h \
|
||||
include/drawstuff/drawstuff.h \
|
||||
include/drawstuff/version.h
|
||||
ode/test/test_boxstack.o: \
|
||||
ode/test/test_boxstack.cpp \
|
||||
include/ode/ode.h \
|
||||
include/ode/config.h \
|
||||
include/ode/contact.h \
|
||||
include/ode/common.h \
|
||||
include/ode/error.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/odemath.h \
|
||||
include/ode/matrix.h \
|
||||
include/ode/timer.h \
|
||||
include/ode/rotation.h \
|
||||
include/ode/mass.h \
|
||||
include/ode/space.h \
|
||||
include/ode/geom.h \
|
||||
include/ode/misc.h \
|
||||
include/ode/objects.h \
|
||||
include/ode/odecpp.h \
|
||||
include/drawstuff/drawstuff.h \
|
||||
include/drawstuff/version.h
|
||||
ode/test/test_buggy.o: \
|
||||
ode/test/test_buggy.cpp \
|
||||
include/ode/ode.h \
|
||||
include/ode/config.h \
|
||||
include/ode/contact.h \
|
||||
include/ode/common.h \
|
||||
include/ode/error.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/odemath.h \
|
||||
include/ode/matrix.h \
|
||||
include/ode/timer.h \
|
||||
include/ode/rotation.h \
|
||||
include/ode/mass.h \
|
||||
include/ode/space.h \
|
||||
include/ode/geom.h \
|
||||
include/ode/misc.h \
|
||||
include/ode/objects.h \
|
||||
include/ode/odecpp.h \
|
||||
include/drawstuff/drawstuff.h \
|
||||
include/drawstuff/version.h
|
||||
ode/test/test_joints.o: \
|
||||
ode/test/test_joints.cpp \
|
||||
include/ode/ode.h \
|
||||
include/ode/config.h \
|
||||
include/ode/contact.h \
|
||||
include/ode/common.h \
|
||||
include/ode/error.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/odemath.h \
|
||||
include/ode/matrix.h \
|
||||
include/ode/timer.h \
|
||||
include/ode/rotation.h \
|
||||
include/ode/mass.h \
|
||||
include/ode/space.h \
|
||||
include/ode/geom.h \
|
||||
include/ode/misc.h \
|
||||
include/ode/objects.h \
|
||||
include/ode/odecpp.h \
|
||||
include/drawstuff/drawstuff.h \
|
||||
include/drawstuff/version.h
|
||||
ode/test/test_space.o: \
|
||||
ode/test/test_space.cpp \
|
||||
include/ode/ode.h \
|
||||
include/ode/config.h \
|
||||
include/ode/contact.h \
|
||||
include/ode/common.h \
|
||||
include/ode/error.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/odemath.h \
|
||||
include/ode/matrix.h \
|
||||
include/ode/timer.h \
|
||||
include/ode/rotation.h \
|
||||
include/ode/mass.h \
|
||||
include/ode/space.h \
|
||||
include/ode/geom.h \
|
||||
include/ode/misc.h \
|
||||
include/ode/objects.h \
|
||||
include/ode/odecpp.h \
|
||||
include/drawstuff/drawstuff.h \
|
||||
include/drawstuff/version.h
|
||||
ode/test/test_I.o: \
|
||||
ode/test/test_I.cpp \
|
||||
include/ode/ode.h \
|
||||
include/ode/config.h \
|
||||
include/ode/contact.h \
|
||||
include/ode/common.h \
|
||||
include/ode/error.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/odemath.h \
|
||||
include/ode/matrix.h \
|
||||
include/ode/timer.h \
|
||||
include/ode/rotation.h \
|
||||
include/ode/mass.h \
|
||||
include/ode/space.h \
|
||||
include/ode/geom.h \
|
||||
include/ode/misc.h \
|
||||
include/ode/objects.h \
|
||||
include/ode/odecpp.h \
|
||||
include/drawstuff/drawstuff.h \
|
||||
include/drawstuff/version.h
|
||||
ode/test/test_step.o: \
|
||||
ode/test/test_step.cpp \
|
||||
include/ode/ode.h \
|
||||
include/ode/config.h \
|
||||
include/ode/contact.h \
|
||||
include/ode/common.h \
|
||||
include/ode/error.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/odemath.h \
|
||||
include/ode/matrix.h \
|
||||
include/ode/timer.h \
|
||||
include/ode/rotation.h \
|
||||
include/ode/mass.h \
|
||||
include/ode/space.h \
|
||||
include/ode/geom.h \
|
||||
include/ode/misc.h \
|
||||
include/ode/objects.h \
|
||||
include/ode/odecpp.h \
|
||||
include/drawstuff/drawstuff.h \
|
||||
include/drawstuff/version.h
|
||||
ode/test/test_friction.o: \
|
||||
ode/test/test_friction.cpp \
|
||||
include/ode/ode.h \
|
||||
include/ode/config.h \
|
||||
include/ode/contact.h \
|
||||
include/ode/common.h \
|
||||
include/ode/error.h \
|
||||
include/ode/memory.h \
|
||||
include/ode/odemath.h \
|
||||
include/ode/matrix.h \
|
||||
include/ode/timer.h \
|
||||
include/ode/rotation.h \
|
||||
include/ode/mass.h \
|
||||
include/ode/space.h \
|
||||
include/ode/geom.h \
|
||||
include/ode/misc.h \
|
||||
include/ode/objects.h \
|
||||
include/ode/odecpp.h \
|
||||
include/drawstuff/drawstuff.h \
|
||||
include/drawstuff/version.h
|
||||
drawstuff/dstest/dstest.o: \
|
||||
drawstuff/dstest/dstest.cpp \
|
||||
include/drawstuff/drawstuff.h \
|
||||
include/drawstuff/version.h
|
||||
30
extern/ode/dist/README
vendored
30
extern/ode/dist/README
vendored
@@ -1,30 +0,0 @@
|
||||
The Open Dynamics Engine (ODE), Copyright (C) 2001,2002 Russell L. Smith.
|
||||
-------------------------------------------------------------------------
|
||||
|
||||
ODE is a free, industrial quality library for simulating articulated
|
||||
rigid body dynamics - for example ground vehicles, legged creatures,
|
||||
and moving objects in VR environments. It is fast, flexible, robust
|
||||
and platform independent, with advanced joints, contact with friction,
|
||||
and built-in collision detection.
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of EITHER:
|
||||
(1) The GNU Lesser General Public License as published by the Free
|
||||
Software Foundation; either version 2.1 of the License, or (at
|
||||
your option) any later version. The text of the GNU Lesser
|
||||
General Public License is included with this library in the
|
||||
file LICENSE.TXT.
|
||||
(2) The BSD-style license that is included with this library in
|
||||
the file LICENSE-BSD.TXT.
|
||||
|
||||
This library is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
|
||||
LICENSE.TXT and LICENSE-BSD.TXT for more details.
|
||||
|
||||
* Installation instructions are in the INSTALL file
|
||||
|
||||
* The ODE web pages are at http://q12.org/ode/
|
||||
|
||||
* The ODE documentation is in the file ode/doc/ode.html, or you
|
||||
can view it on the web at http://q12.org/ode/ode-docs.html
|
||||
18
extern/ode/dist/README_BLENDER
vendored
18
extern/ode/dist/README_BLENDER
vendored
@@ -1,18 +0,0 @@
|
||||
Checked out from ODE cvs (http://q12.org) on or around
|
||||
Fri Oct 18 14:24:11 GMT 2002
|
||||
|
||||
Note that ODE has its own build system. The source/ode/
|
||||
directory is traversed by Blender's source/Makefile. However
|
||||
source/ode/config/user-settings has to be set correctly
|
||||
depending on the target platform. This needs to be done in the
|
||||
source/Makefile which determines the proper platform, then copies
|
||||
the appropriate platform-specific user-settings.platform file to
|
||||
source/ode/config/user-settings. Currently source/ode/user-settings is
|
||||
for Linux.
|
||||
|
||||
Don't change the source in this directory. This source code is
|
||||
maintained on the CVS server at http://q12.org and is provided here
|
||||
so that Blender developers can compile ODE without having to separately
|
||||
download and install it. This source code can and should periodically
|
||||
be synchronized (manually) with the source code at http://q12.org.
|
||||
|
||||
41
extern/ode/dist/config/README
vendored
41
extern/ode/dist/config/README
vendored
@@ -1,41 +0,0 @@
|
||||
|
||||
variable names used in the per-platform makefile configuration files:
|
||||
|
||||
platform stuff
|
||||
--------------
|
||||
|
||||
WINDOWS set to 1 if this is a microsoft windows based platform
|
||||
|
||||
filesystem stuff and commands
|
||||
-----------------------------
|
||||
|
||||
THIS_DIR prefix to run a command from the current directory
|
||||
DEL_CMD the name of the delete command
|
||||
|
||||
compiler stuff
|
||||
--------------
|
||||
|
||||
CC the C/C++ compiler to use
|
||||
OBJ the object file extension
|
||||
C_FLAGS the standard set of compiler flags
|
||||
C_INC flag to add an include path
|
||||
C_OUT flag to specify the object file output
|
||||
C_EXEOUT flag to specify the executable file output
|
||||
C_DEF flag to add a define
|
||||
C_OPT flag to set the optimization level
|
||||
OPT the optimization level to use
|
||||
|
||||
library archiver
|
||||
----------------
|
||||
|
||||
AR library archiver command
|
||||
RANLIB ranlib command, if necessary
|
||||
LIB_PREFIX library file prefix
|
||||
LIB_SUFFIX library file suffix
|
||||
LINK_OPENGL link flags to link in windowing stuff and opengl
|
||||
LINK_MATH link flags to link in the system math library
|
||||
|
||||
windows specific stuff
|
||||
----------------------
|
||||
|
||||
RC_RULE makefile rule to use for the resource compiler
|
||||
28
extern/ode/dist/config/makefile.cygwin
vendored
28
extern/ode/dist/config/makefile.cygwin
vendored
@@ -1,28 +0,0 @@
|
||||
WINDOWS=1
|
||||
THIS_DIR=./
|
||||
DEL_CMD=rm -f
|
||||
CC=gcc
|
||||
OBJ=.o
|
||||
C_FLAGS=-c -Wall -fno-exceptions -fno-rtti -DWIN32 -DCYGWIN
|
||||
C_INC=-I
|
||||
C_OUT=-o
|
||||
C_EXEOUT=-o
|
||||
C_DEF=-D
|
||||
C_OPT=-O
|
||||
AR=ar rc
|
||||
RANLIB=
|
||||
LIB_PREFIX=lib
|
||||
LIB_SUFFIX=.a
|
||||
LINK_OPENGL=-lComctl32 -lkernel32 -luser32 -lgdi32 -lOpenGL32 -lGlu32
|
||||
LINK_MATH=-lm
|
||||
RC_RULE=windres -I rc -O coff $< $@
|
||||
|
||||
ifeq ($(BUILD),release)
|
||||
OPT=2
|
||||
C_FLAGS+=-fomit-frame-pointer -ffast-math
|
||||
endif
|
||||
|
||||
ifeq ($(BUILD),debug)
|
||||
OPT=0
|
||||
C_FLAGS+=-g
|
||||
endif
|
||||
28
extern/ode/dist/config/makefile.mingw
vendored
28
extern/ode/dist/config/makefile.mingw
vendored
@@ -1,28 +0,0 @@
|
||||
WINDOWS=1
|
||||
THIS_DIR=
|
||||
DEL_CMD=tools\rm
|
||||
CC=gcc
|
||||
OBJ=.o
|
||||
C_FLAGS=-c -Wall -fno-exceptions -fno-rtti -DWIN32
|
||||
C_INC=-I
|
||||
C_OUT=-o
|
||||
C_EXEOUT=-o
|
||||
C_DEF=-D
|
||||
C_OPT=-O
|
||||
AR=ar rc
|
||||
RANLIB=
|
||||
LIB_PREFIX=lib
|
||||
LIB_SUFFIX=.a
|
||||
LINK_OPENGL=-lComctl32 -lkernel32 -luser32 -lgdi32 -lOpenGL32 -lGlu32
|
||||
LINK_MATH=-lm
|
||||
RC_RULE=windres -I rc -O coff $< $@
|
||||
|
||||
ifeq ($(BUILD),release)
|
||||
OPT=2
|
||||
C_FLAGS+=-fomit-frame-pointer -ffast-math
|
||||
endif
|
||||
|
||||
ifeq ($(BUILD),debug)
|
||||
OPT=0
|
||||
C_FLAGS+=-g
|
||||
endif
|
||||
27
extern/ode/dist/config/makefile.msvc
vendored
27
extern/ode/dist/config/makefile.msvc
vendored
@@ -1,27 +0,0 @@
|
||||
WINDOWS=1
|
||||
THIS_DIR=
|
||||
DEL_CMD=tools\rm
|
||||
CC=cl /nologo /DWIN32 /DMSVC /DSHAREDLIBEXPORT= /DSHAREDLIBIMPORT=
|
||||
OBJ=.obj
|
||||
C_FLAGS=/c /GR- /GX- /W3 /GF
|
||||
C_INC=/I
|
||||
C_OUT=/Fo
|
||||
C_EXEOUT=/Fe
|
||||
C_DEF=/D
|
||||
C_OPT=/O
|
||||
AR=lib /nologo /OUT:
|
||||
RANLIB=
|
||||
LIB_PREFIX=
|
||||
LIB_SUFFIX=.lib
|
||||
LINK_OPENGL=Comctl32.lib kernel32.lib user32.lib gdi32.lib OpenGL32.lib Glu32.lib
|
||||
LINK_MATH=
|
||||
RC_RULE=rc /r /fo$@ $<
|
||||
|
||||
ifeq ($(BUILD),release)
|
||||
OPT=2
|
||||
C_FLAGS+=/Oy
|
||||
endif
|
||||
|
||||
ifeq ($(BUILD),debug)
|
||||
OPT=d
|
||||
endif
|
||||
29
extern/ode/dist/config/makefile.msvc-dll
vendored
29
extern/ode/dist/config/makefile.msvc-dll
vendored
@@ -1,29 +0,0 @@
|
||||
WINDOWS=1
|
||||
THIS_DIR=
|
||||
DEL_CMD=tools\rm
|
||||
CC=cl /nologo /DWIN32 /DMSVC /DSHAREDLIBIMPORT=__declspec(dllimport) /DSHAREDLIBEXPORT=__declspec(dllexport)
|
||||
OBJ=.obj
|
||||
C_FLAGS=/c /GR- /GX- /W3 /GF
|
||||
C_INC=/I
|
||||
C_OUT=/Fo
|
||||
C_EXEOUT=/Fe
|
||||
C_DEF=/D
|
||||
C_OPT=/O
|
||||
AR=lib /nologo /OUT:
|
||||
RANLIB=
|
||||
LIB_PREFIX=
|
||||
LIB_SUFFIX=.lib
|
||||
LINK_OPENGL=Comctl32.lib kernel32.lib user32.lib gdi32.lib OpenGL32.lib Glu32.lib
|
||||
LINK_MATH=
|
||||
RC_RULE=rc /r /fo$@ $<
|
||||
|
||||
ifeq ($(BUILD),release)
|
||||
OPT=2
|
||||
C_FLAGS+=/Oy
|
||||
endif
|
||||
|
||||
ifeq ($(BUILD),debug)
|
||||
OPT=d
|
||||
endif
|
||||
|
||||
ODE_LIB_AR_RULE=link /dll /nologo /SUBSYSTEM:WINDOWS /LIBPATH:"C:\Programme\Micros~2\VC98\Lib" /def:config/msvcdefs.def $(LINK_OPENGL) /OUT:$(patsubst %.lib,%.dll,$@)
|
||||
26
extern/ode/dist/config/makefile.osx
vendored
26
extern/ode/dist/config/makefile.osx
vendored
@@ -1,26 +0,0 @@
|
||||
THIS_DIR=./
|
||||
DEL_CMD=rm -f
|
||||
CC=cc
|
||||
OBJ=.o
|
||||
C_FLAGS=-c -Wall -fno-rtti -fno-exceptions -Wall -I/usr/X11R6/include
|
||||
C_INC=-I
|
||||
C_OUT=-o
|
||||
C_EXEOUT=-o
|
||||
C_DEF=-D
|
||||
C_OPT=-O
|
||||
AR=ar rc
|
||||
RANLIB=ranlib
|
||||
LIB_PREFIX=lib
|
||||
LIB_SUFFIX=.a
|
||||
LINK_OPENGL=-L/usr/X11R6/lib -L/usr/X11/lib -L/usr/lib/X11R6 -L/usr/lib/X11 -lX11 -lGL -lGLU -lstdc++
|
||||
LINK_MATH=-lm
|
||||
|
||||
ifeq ($(BUILD),release)
|
||||
OPT=2
|
||||
C_FLAGS+=-fomit-frame-pointer -ffast-math
|
||||
endif
|
||||
|
||||
ifeq ($(BUILD),debug)
|
||||
OPT=0
|
||||
C_FLAGS+=-g
|
||||
endif
|
||||
29
extern/ode/dist/config/makefile.unix-gcc
vendored
29
extern/ode/dist/config/makefile.unix-gcc
vendored
@@ -1,29 +0,0 @@
|
||||
THIS_DIR=./
|
||||
DEL_CMD=rm -f
|
||||
CC=gcc
|
||||
OBJ=.o
|
||||
C_FLAGS=-c -Wall -fno-rtti -fno-exceptions -Wall
|
||||
C_INC=-I
|
||||
C_OUT=-o
|
||||
C_EXEOUT=-o
|
||||
C_DEF=-D
|
||||
C_OPT=-O
|
||||
AR=ar rc
|
||||
RANLIB=
|
||||
LIB_PREFIX=lib
|
||||
LIB_SUFFIX=.a
|
||||
LINK_OPENGL=-L/usr/X11R6/lib -L/usr/X11/lib -L/usr/lib/X11R6 -L/usr/lib/X11 -lX11 -lGL -lGLU
|
||||
LINK_MATH=-lm
|
||||
|
||||
ifeq ($(BUILD),release)
|
||||
OPT=2
|
||||
C_FLAGS+=-fomit-frame-pointer -ffast-math
|
||||
endif
|
||||
|
||||
ifeq ($(BUILD),debug)
|
||||
OPT=0
|
||||
C_FLAGS+=-g
|
||||
endif
|
||||
|
||||
# some other possible flags:
|
||||
# -malign-double -mpentiumpro -march=pentiumpro
|
||||
24
extern/ode/dist/config/makefile.unix-generic
vendored
24
extern/ode/dist/config/makefile.unix-generic
vendored
@@ -1,24 +0,0 @@
|
||||
THIS_DIR=./
|
||||
DEL_CMD=rm -f
|
||||
CC=CC
|
||||
OBJ=.o
|
||||
C_FLAGS=-c
|
||||
C_INC=-I
|
||||
C_OUT=-o
|
||||
C_EXEOUT=-o
|
||||
C_DEF=-D
|
||||
C_OPT=-O
|
||||
AR=ar rc
|
||||
RANLIB=
|
||||
LIB_PREFIX=lib
|
||||
LIB_SUFFIX=.a
|
||||
LINK_OPENGL=-L/usr/X11R6/lib -L/usr/X11/lib -L/usr/lib/X11R6 -L/usr/lib/X11 -lX11 -lGL -lGLU
|
||||
LINK_MATH=-lm
|
||||
|
||||
ifeq ($(BUILD),release)
|
||||
OPT=2
|
||||
endif
|
||||
|
||||
ifeq ($(BUILD),debug)
|
||||
OPT=0
|
||||
endif
|
||||
228
extern/ode/dist/config/msvcdefs.def
vendored
228
extern/ode/dist/config/msvcdefs.def
vendored
@@ -1,228 +0,0 @@
|
||||
LIBRARY ODE
|
||||
EXPORTS
|
||||
dAreConnected
|
||||
dBodyAddForce
|
||||
dBodyAddForceAtPos
|
||||
dBodyAddForceAtRelPos
|
||||
dBodyAddRelForce
|
||||
dBodyAddRelForceAtPos
|
||||
dBodyAddRelForceAtRelPos
|
||||
dBodyAddRelTorque
|
||||
dBodyAddTorque
|
||||
dBodyCreate
|
||||
dBodyDestroy
|
||||
dBodyDisable
|
||||
dBodyEnable
|
||||
dBodyGetAngularVel
|
||||
dBodyGetData
|
||||
dBodyGetFiniteRotationAxis
|
||||
dBodyGetFiniteRotationMode
|
||||
dBodyGetForce
|
||||
dBodyGetGravityMode
|
||||
dBodyGetJoint
|
||||
dBodyGetLinearVel
|
||||
dBodyGetMass
|
||||
dBodyGetNumJoints
|
||||
dBodyGetPointVel
|
||||
dBodyGetPosRelPoint
|
||||
dBodyGetPosition
|
||||
dBodyGetQuaternion
|
||||
dBodyGetRelPointPos
|
||||
dBodyGetRelPointVel
|
||||
dBodyGetRotation
|
||||
dBodyGetTorque
|
||||
dBodyIsEnabled
|
||||
dBodySetAngularVel
|
||||
dBodySetData
|
||||
dBodySetFiniteRotationAxis
|
||||
dBodySetFiniteRotationMode
|
||||
dBodySetForce
|
||||
dBodySetGravityMode
|
||||
dBodySetLinearVel
|
||||
dBodySetMass
|
||||
dBodySetPosition
|
||||
dBodySetQuaternion
|
||||
dBodySetRotation
|
||||
dBodySetTorque
|
||||
dBodyVectorFromWorld
|
||||
dBodyVectorToWorld
|
||||
dBoxBox
|
||||
dBoxClass
|
||||
dBoxTouchesBox
|
||||
dCCylinderClass
|
||||
dClearUpperTriangle
|
||||
dCloseODE
|
||||
dClosestLineSegmentPoints
|
||||
dCollide
|
||||
dCreateBox
|
||||
dCreateCCylinder
|
||||
dCreateGeom
|
||||
dCreateGeomClass
|
||||
dCreateGeomGroup
|
||||
dCreateGeomTransform
|
||||
dCreatePlane
|
||||
dCreateSphere
|
||||
dError
|
||||
dFactorCholesky
|
||||
dFactorLDLT
|
||||
dGeomBoxGetLengths
|
||||
dGeomBoxSetLengths
|
||||
dGeomCCylinderGetParams
|
||||
dGeomCCylinderSetParams
|
||||
dGeomDestroy
|
||||
dGeomGetAABB
|
||||
dGeomGetBody
|
||||
dGeomGetClass
|
||||
dGeomGetClassData
|
||||
dGeomGetData
|
||||
dGeomGetPosition
|
||||
dGeomGetRotation
|
||||
dGeomGetSpaceAABB
|
||||
dGeomGroupAdd
|
||||
dGeomGroupGetGeom
|
||||
dGeomGroupGetNumGeoms
|
||||
dGeomGroupRemove
|
||||
dGeomPlaneGetParams
|
||||
dGeomPlaneSetParams
|
||||
dGeomSetBody
|
||||
dGeomSetData
|
||||
dGeomSetPosition
|
||||
dGeomSetRotation
|
||||
dGeomSphereGetRadius
|
||||
dGeomSphereSetRadius
|
||||
dGeomTransformClass
|
||||
dGeomTransformGetCleanup
|
||||
dGeomTransformGetGeom
|
||||
dGeomTransformSetCleanup
|
||||
dGeomTransformSetGeom
|
||||
dHashSpaceCreate
|
||||
dHashSpaceSetLevels
|
||||
dInfiniteAABB
|
||||
dInfinityValue
|
||||
dInvertPDMatrix
|
||||
dIsPositiveDefinite
|
||||
dJointAttach
|
||||
dJointCreateAMotor
|
||||
dJointCreateBall
|
||||
dJointCreateContact
|
||||
dJointCreateFixed
|
||||
dJointCreateHinge
|
||||
dJointCreateHinge2
|
||||
dJointCreateSlider
|
||||
dJointCreateUniversal
|
||||
dJointDestroy
|
||||
dJointGetAMotorAngle
|
||||
dJointGetAMotorAngleRate
|
||||
dJointGetAMotorAxis
|
||||
dJointGetAMotorAxisRel
|
||||
dJointGetAMotorMode
|
||||
dJointGetAMotorNumAxes
|
||||
dJointGetAMotorParam
|
||||
dJointGetBallAnchor
|
||||
dJointGetBody
|
||||
dJointGetData
|
||||
dJointGetHinge2Anchor
|
||||
dJointGetHinge2Angle1
|
||||
dJointGetHinge2Angle1Rate
|
||||
dJointGetHinge2Angle2Rate
|
||||
dJointGetHinge2Axis1
|
||||
dJointGetHinge2Axis2
|
||||
dJointGetHinge2Param
|
||||
dJointGetHingeAnchor
|
||||
dJointGetHingeAngle
|
||||
dJointGetHingeAngleRate
|
||||
dJointGetHingeAxis
|
||||
dJointGetHingeParam
|
||||
dJointGetSliderAxis
|
||||
dJointGetSliderParam
|
||||
dJointGetSliderPosition
|
||||
dJointGetSliderPositionRate
|
||||
dJointGetType
|
||||
dJointGetUniversalAnchor
|
||||
dJointGetUniversalAxis1
|
||||
dJointGetUniversalAxis2
|
||||
dJointGroupCreate
|
||||
dJointGroupDestroy
|
||||
dJointGroupEmpty
|
||||
dJointSetAMotorAngle
|
||||
dJointSetAMotorAxis
|
||||
dJointSetAMotorMode
|
||||
dJointSetAMotorNumAxes
|
||||
dJointSetAMotorParam
|
||||
dJointSetBallAnchor
|
||||
dJointSetData
|
||||
dJointSetFixed
|
||||
dJointSetHinge2Anchor
|
||||
dJointSetHinge2Axis1
|
||||
dJointSetHinge2Axis2
|
||||
dJointSetHinge2Param
|
||||
dJointSetHingeAnchor
|
||||
dJointSetHingeAxis
|
||||
dJointSetHingeParam
|
||||
dJointSetSliderAxis
|
||||
dJointSetSliderParam
|
||||
dJointSetUniversalAnchor
|
||||
dJointSetUniversalAxis1
|
||||
dJointSetUniversalAxis2
|
||||
dLDLTAddTL
|
||||
dLDLTRemove
|
||||
dMakeRandomMatrix
|
||||
dMakeRandomVector
|
||||
dMassAdd
|
||||
dMassAdjust
|
||||
dMassRotate
|
||||
dMassSetBox
|
||||
dMassSetCappedCylinder
|
||||
dMassSetParameters
|
||||
dMassSetSphere
|
||||
dMassSetZero
|
||||
dMassTranslate
|
||||
dMaxDifference
|
||||
dMultiply0
|
||||
dMultiply1
|
||||
dMultiply2
|
||||
dNormalize3
|
||||
dNormalize4
|
||||
dPlaneSpace
|
||||
dQFromAxisAndAngle
|
||||
dQMultiply0
|
||||
dQMultiply1
|
||||
dQMultiply2
|
||||
dQMultiply3
|
||||
dQSetIdentity
|
||||
dQtoR
|
||||
dRFrom2Axes
|
||||
dRFromAxisAndAngle
|
||||
dRFromEulerAngles
|
||||
dRSetIdentity
|
||||
dRandInt
|
||||
dRandReal
|
||||
dRandSetSeed
|
||||
dRemoveRowCol
|
||||
dRtoQ
|
||||
dSetMessageHandler
|
||||
dSetZero
|
||||
dSimpleSpaceCreate
|
||||
dSolveCholesky
|
||||
dSolveLDLT
|
||||
dSpaceAdd
|
||||
dSpaceCollide
|
||||
dSpaceDestroy
|
||||
dSpaceQuery
|
||||
dSpaceRemove
|
||||
dSphereClass
|
||||
dTestMatrixComparison
|
||||
dTestRand
|
||||
dTestSolveLCP
|
||||
dWorldCreate
|
||||
dWorldDestroy
|
||||
dWorldGetCFM
|
||||
dWorldGetERP
|
||||
dWorldGetGravity
|
||||
dWorldImpulseToForce
|
||||
dWorldSetCFM
|
||||
dWorldSetERP
|
||||
dWorldSetGravity
|
||||
dWorldStep
|
||||
dWorldStep
|
||||
dWtoDQ
|
||||
31
extern/ode/dist/config/user-settings
vendored
31
extern/ode/dist/config/user-settings
vendored
@@ -1,31 +0,0 @@
|
||||
# ODE user settings: the following variables must be set by the user
|
||||
|
||||
# (1) the platform to use. this name should have a corresponding
|
||||
# makefile.PLATFORM file. currently supported platforms are:
|
||||
# msvc microsoft visual C/C++
|
||||
# msvc-dll microsoft visual C/C++, create a DLL
|
||||
# mingw minimalist GNU for windows
|
||||
# cygwin cygnus GNU for windows
|
||||
# unix-gcc GNU gcc on unix
|
||||
# unix-generic generic unix compiler. you may need to edit the CC
|
||||
# variable in makefile.unix-generic
|
||||
# osx Mac OS-X, with the gnu compiler.
|
||||
|
||||
PLATFORM=unix-gcc
|
||||
|
||||
# (2) the floating point precision to use (either "SINGLE" or "DOUBLE")
|
||||
|
||||
PRECISION=SINGLE
|
||||
#PRECISION=DOUBLE
|
||||
|
||||
# (3) the library type to build (either "debug" if you are doing development,
|
||||
# or "release" for the optimized library)
|
||||
|
||||
#BUILD=debug
|
||||
BUILD=release
|
||||
|
||||
# (4) if you are using an old version of MS-Windows that has command line
|
||||
# length limitations then you will need to set this to "1". otherwise,
|
||||
# leave it at "0".
|
||||
|
||||
WINDOWS16=0
|
||||
31
extern/ode/dist/config/user-settings.example
vendored
31
extern/ode/dist/config/user-settings.example
vendored
@@ -1,31 +0,0 @@
|
||||
# ODE user settings: the following variables must be set by the user
|
||||
|
||||
# (1) the platform to use. this name should have a corresponding
|
||||
# makefile.PLATFORM file. currently supported platforms are:
|
||||
# msvc microsoft visual C/C++
|
||||
# msvc-dll microsoft visual C/C++, create a DLL
|
||||
# mingw minimalist GNU for windows
|
||||
# cygwin cygnus GNU for windows
|
||||
# unix-gcc GNU gcc on unix
|
||||
# unix-generic generic unix compiler. you may need to edit the CC
|
||||
# variable in makefile.unix-generic
|
||||
# osx Mac OS-X, with the gnu compiler.
|
||||
|
||||
PLATFORM=unix-gcc
|
||||
|
||||
# (2) the floating point precision to use (either "SINGLE" or "DOUBLE")
|
||||
|
||||
#PRECISION=SINGLE
|
||||
PRECISION=DOUBLE
|
||||
|
||||
# (3) the library type to build (either "debug" if you are doing development,
|
||||
# or "release" for the optimized library)
|
||||
|
||||
#BUILD=debug
|
||||
BUILD=release
|
||||
|
||||
# (4) if you are using an old version of MS-Windows that has command line
|
||||
# length limitations then you will need to set this to "1". otherwise,
|
||||
# leave it at "0".
|
||||
|
||||
WINDOWS16=0
|
||||
437
extern/ode/dist/configurator.c
vendored
437
extern/ode/dist/configurator.c
vendored
@@ -1,437 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
/*
|
||||
|
||||
this program discovers some system configuration stuff, prior to compiling
|
||||
ODE. the usage is:
|
||||
|
||||
configurator <config.h-file-to-generate> <compiler-command-line>
|
||||
<delete-command-line> <THIS_DIR-variable>
|
||||
|
||||
this program looks long, but it really has an extremely simple structure and
|
||||
should be very easy for anyone to modify. it should be very portable as it
|
||||
is written in straight ANSI C and only uses the following library functions:
|
||||
* printf
|
||||
* fopen (assumes 0 returned on failure)
|
||||
* fclose
|
||||
* fprintf
|
||||
* system
|
||||
* exit
|
||||
except where stated, we do not assume anything about the return codes from
|
||||
these functions.
|
||||
|
||||
why didn't i just use GNU autoconf? :
|
||||
* autoconf needs a bourne shell and a bunch of other tools that windows
|
||||
users may not have.
|
||||
* i like reinventing the wheel.
|
||||
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
/****************************************************************************/
|
||||
/* project constants */
|
||||
|
||||
#define SETUP_SHLIB_DEFS \
|
||||
"#ifndef SHAREDLIBIMPORT\n" \
|
||||
"#define SHAREDLIBIMPORT\n" \
|
||||
"#endif\n" \
|
||||
"#ifndef SHAREDLIBEXPORT\n" \
|
||||
"#define SHAREDLIBEXPORT\n" \
|
||||
"#endif\n"
|
||||
|
||||
/* the config.h header */
|
||||
char *config_h_part1 =
|
||||
"/* per-machine configuration. this file is automatically generated. */\n"
|
||||
"\n"
|
||||
"#ifndef _ODE_CONFIG_H_\n"
|
||||
"#define _ODE_CONFIG_H_\n"
|
||||
"\n"
|
||||
"/* shared lib definitions */\n"
|
||||
SETUP_SHLIB_DEFS
|
||||
"\n"
|
||||
"/* standard system headers */\n";
|
||||
|
||||
|
||||
char *config_h_part2 =
|
||||
"\n"
|
||||
"#ifdef __cplusplus\n"
|
||||
"extern \"C\" {\n"
|
||||
"#endif\n"
|
||||
"\n";
|
||||
|
||||
/* the config.h footer */
|
||||
char *config_h_footer =
|
||||
"#ifdef __cplusplus\n"
|
||||
"}\n"
|
||||
"#endif\n"
|
||||
"#endif\n";
|
||||
|
||||
/****************************************************************************/
|
||||
/* implementations of some string functions. these are prefixed with 'x'
|
||||
* to prevent any conflicts with built-in functions.
|
||||
*/
|
||||
|
||||
#define strcpy xstrcpy
|
||||
void xstrcpy (char *dest, char *src)
|
||||
{
|
||||
while (*src) *dest++ = *src++;
|
||||
*dest = 0;
|
||||
}
|
||||
|
||||
|
||||
#define strcat xstrcat
|
||||
void xstrcat (char *dest, char *src)
|
||||
{
|
||||
while (*dest) dest++;
|
||||
while (*src) *dest++ = *src++;
|
||||
*dest = 0;
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
/* utility functions */
|
||||
|
||||
/* print an error message and exit */
|
||||
|
||||
void fatal_error (char *message)
|
||||
{
|
||||
printf ("\n*** configurator failed: %s.\n\n"
|
||||
"please fix your configuration and try again.\n"
|
||||
"if you have to fix the configurator program or the makefiles, "
|
||||
"please email\n"
|
||||
"your changes to the ODE mailing list (ode@q12.org).\n\n", message);
|
||||
exit (0);
|
||||
}
|
||||
|
||||
|
||||
/* open a file, generate an error if it can't be done */
|
||||
|
||||
FILE * xfopen (char *filename, char *mode)
|
||||
{
|
||||
FILE *f;
|
||||
f = fopen (filename,mode);
|
||||
if (!f) fatal_error ("can not open a file");
|
||||
return f;
|
||||
}
|
||||
|
||||
|
||||
/* return 1 if the file exists or 0 if not */
|
||||
|
||||
int file_exists (char *filename)
|
||||
{
|
||||
FILE *f;
|
||||
f = fopen (filename,"rb");
|
||||
if (f) fclose (f);
|
||||
return (f != 0);
|
||||
}
|
||||
|
||||
|
||||
/* write a string to a new file */
|
||||
|
||||
void write_to_file (char *filename, char *s)
|
||||
{
|
||||
FILE *f = xfopen (filename,"wt");
|
||||
fprintf (f,"%s",s);
|
||||
fclose (f);
|
||||
}
|
||||
|
||||
|
||||
/* write a comment to a header file */
|
||||
|
||||
void write_header_comment (FILE *file, char *description)
|
||||
{
|
||||
fprintf (file,"/* %s */\n",description);
|
||||
printf ("%s ...\n",description);
|
||||
}
|
||||
|
||||
|
||||
/* delete a file */
|
||||
|
||||
char *delete_cmd_line = 0;
|
||||
void delete_file (char *filename)
|
||||
{
|
||||
char cmd[1000];
|
||||
strcpy (cmd,delete_cmd_line);
|
||||
strcat (cmd," ");
|
||||
strcat (cmd,filename);
|
||||
printf ("%s\n",cmd);
|
||||
system (cmd);
|
||||
}
|
||||
|
||||
|
||||
/* run a compile command */
|
||||
|
||||
char *compile_cmd_line = 0;
|
||||
void compile (char *output, char *input)
|
||||
{
|
||||
char cmd[1000];
|
||||
strcpy (cmd,compile_cmd_line);
|
||||
strcat (cmd,output);
|
||||
strcat (cmd," ");
|
||||
strcat (cmd,input);
|
||||
printf ("%s\n",cmd);
|
||||
system (cmd);
|
||||
}
|
||||
|
||||
|
||||
/* run a program we've just compiled */
|
||||
|
||||
char *run_prefix = "";
|
||||
void run (char *filename)
|
||||
{
|
||||
char cmd[1000];
|
||||
strcpy (cmd,run_prefix);
|
||||
strcat (cmd,filename);
|
||||
printf ("%s\n",cmd);
|
||||
system (cmd);
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
/* system tests */
|
||||
|
||||
void check_if_this_is_a_pentium (FILE *file)
|
||||
{
|
||||
write_header_comment (file,"is this a pentium on a gcc-based platform?");
|
||||
write_to_file ("ctest.c",
|
||||
"int main() {\n"
|
||||
" asm (\"mov $0,%%eax\\n cpuid\\n\" : : : \"%eax\");\n"
|
||||
" return 0;\n"
|
||||
"}\n");
|
||||
delete_file ("ctest.exe");
|
||||
compile ("ctest.exe","ctest.c");
|
||||
if (file_exists ("ctest.exe")) {
|
||||
fprintf (file,"#define PENTIUM 1\n\n");
|
||||
}
|
||||
else {
|
||||
fprintf (file,"/* #define PENTIUM 1 -- not a pentium */\n\n");
|
||||
}
|
||||
|
||||
delete_file ("ctest.c");
|
||||
delete_file ("ctest.exe");
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
/* tests: standard headers */
|
||||
|
||||
void get_all_standard_headers (FILE *file)
|
||||
{
|
||||
int i;
|
||||
FILE *f;
|
||||
char *header[7] = {"stdio.h", "stdlib.h", "math.h", "string.h",
|
||||
"stdarg.h", "malloc.h", "alloca.h"};
|
||||
|
||||
for (i=0; i < sizeof(header)/sizeof(char*); i++) {
|
||||
FILE *f = xfopen ("ctest.c","wt");
|
||||
fprintf (f,"#include <%s>\nint main() { return 0; }\n",header[i]);
|
||||
fclose (f);
|
||||
delete_file ("ctest.exe");
|
||||
compile ("ctest.exe","ctest.c");
|
||||
if (file_exists ("ctest.exe")) {
|
||||
fprintf (file,"#include <%s>\n",header[i]);
|
||||
}
|
||||
}
|
||||
|
||||
delete_file ("ctest.c");
|
||||
delete_file ("ctest.exe");
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
/* tests: typedefs and constants for ODE */
|
||||
|
||||
void get_ODE_integer_typedefs (FILE *file)
|
||||
{
|
||||
write_header_comment (file,"integer types (we assume int >= 32 bits)");
|
||||
if (sizeof(char) != 1) fatal_error ("expecting sizeof(char) == 1");
|
||||
if (sizeof(int) < 4) fatal_error ("expecting sizeof(int) >= 4");
|
||||
fprintf (file,"typedef char int8;\ntypedef unsigned char uint8;\n");
|
||||
if (sizeof(short) == 4) {
|
||||
fprintf (file,"typedef short int32;\ntypedef unsigned short uint32;\n");
|
||||
}
|
||||
else if (sizeof(int) == 4) {
|
||||
fprintf (file,"typedef int int32;\ntypedef unsigned int uint32;\n");
|
||||
}
|
||||
else {
|
||||
fatal_error ("can not find 4 byte integer type");
|
||||
}
|
||||
fprintf (file,"\n"
|
||||
"/* an integer type that we can safely cast a pointer to and\n"
|
||||
" * from without loss of bits.\n"
|
||||
" */\n");
|
||||
if (sizeof(short) == sizeof(void*)) {
|
||||
fprintf (file,"typedef unsigned short intP;\n");
|
||||
}
|
||||
else if (sizeof(int) == sizeof(void*)) {
|
||||
fprintf (file,"typedef unsigned int intP;\n");
|
||||
}
|
||||
else if (sizeof(long int) == sizeof(void*)) {
|
||||
fprintf (file,"typedef unsigned long int intP;\n");
|
||||
}
|
||||
fprintf (file,"\n");
|
||||
}
|
||||
|
||||
|
||||
void get_ODE_float_stuff (FILE *file)
|
||||
{
|
||||
char *suffix,*type;
|
||||
int i;
|
||||
FILE *f;
|
||||
|
||||
#define SHARED_LIB_SPEC_DECISION \
|
||||
"#if defined SHARED_CONFIG_H_INCLUDED_FROM_DEFINING_FILE\n" \
|
||||
" #define GLOBAL_SHAREDLIB_SPEC SHAREDLIBEXPORT\n" \
|
||||
"#else \n" \
|
||||
" #define GLOBAL_SHAREDLIB_SPEC SHAREDLIBIMPORT\n" \
|
||||
"#endif\n"
|
||||
|
||||
#define UNDEF_SHAREDLIB_SPEC "\n#undef GLOBAL_SHAREDLIB_SPEC\n"
|
||||
|
||||
#ifdef dSINGLE
|
||||
|
||||
#define INFBYTES SHARED_LIB_SPEC_DECISION "union dInfBytes { unsigned char c[4]; float f; };\nextern GLOBAL_SHAREDLIB_SPEC union dInfBytes dInfinityValue;\n#define dInfinity (dInfinityValue.f)"
|
||||
|
||||
char *inc[6] = {"#include <math.h>",
|
||||
"#include <math.h>",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
""};
|
||||
char *decl[6] = {
|
||||
"SHAREDLIBEXPORT double dInfinityValue = HUGE_VALF;",
|
||||
"SHAREDLIBEXPORT double dInfinityValue = HUGE_VAL;",
|
||||
"SHAREDLIBEXPORT union dInfBytes dInfinityValue = {{0x7f,0x80,0,0}};",
|
||||
"SHAREDLIBEXPORT union dInfBytes dInfinityValue = {{0,0,0x80,0x7f}};",
|
||||
"SHAREDLIBEXPORT double dInfinityValue = 1.0f/0.0f;",
|
||||
"SHAREDLIBEXPORT double dInfinityValue = 1e20f;"};
|
||||
char *inf[6] = {
|
||||
SHARED_LIB_SPEC_DECISION "extern GLOBAL_SHAREDLIB_SPEC double dInfinityValue;\n#define dInfinity dInfinityValue" UNDEF_SHAREDLIB_SPEC,
|
||||
SHARED_LIB_SPEC_DECISION "extern GLOBAL_SHAREDLIB_SPEC double dInfinityValue;\n#define dInfinity dInfinityValue" UNDEF_SHAREDLIB_SPEC,
|
||||
INFBYTES UNDEF_SHAREDLIB_SPEC,
|
||||
INFBYTES UNDEF_SHAREDLIB_SPEC,
|
||||
SHARED_LIB_SPEC_DECISION "extern GLOBAL_SHAREDLIB_SPEC double dInfinityValue;\n#define dInfinity dInfinityValue" UNDEF_SHAREDLIB_SPEC,
|
||||
SHARED_LIB_SPEC_DECISION "extern GLOBAL_SHAREDLIB_SPEC double dInfinityValue;\n#define dInfinity dInfinityValue" UNDEF_SHAREDLIB_SPEC};
|
||||
|
||||
#else /* not dSINGLE, must be dDOUBLE */
|
||||
|
||||
#define INFBYTES SHARED_LIB_SPEC_DECISION "union dInfBytes { unsigned char c[8]; double d; };\nextern GLOBAL_SHAREDLIB_SPEC union dInfBytes dInfinityValue;\n#define dInfinity (dInfinityValue.d)"
|
||||
|
||||
char *inc[5] = {
|
||||
"#include <math.h>",
|
||||
"",
|
||||
"",
|
||||
"",
|
||||
""};
|
||||
char *decl[5] = {
|
||||
"SHAREDLIBEXPORT double dInfinityValue = HUGE_VAL;",
|
||||
"SHAREDLIBEXPORT union dInfBytes dInfinityValue = {{0x7f,0xf0,0,0,0,0,0,0}};",
|
||||
"SHAREDLIBEXPORT union dInfBytes dInfinityValue = {{0,0,0,0,0,0,0xf0,0x7f}};",
|
||||
"SHAREDLIBEXPORT double dInfinityValue = 1.0/0.0;",
|
||||
"SHAREDLIBEXPORT double dInfinityValue = 1e20;"};
|
||||
char *inf[5] = {
|
||||
SHARED_LIB_SPEC_DECISION "extern GLOBAL_SHAREDLIB_SPEC double dInfinityValue;\n#define dInfinity dInfinityValue" UNDEF_SHAREDLIB_SPEC,
|
||||
INFBYTES UNDEF_SHAREDLIB_SPEC,
|
||||
INFBYTES UNDEF_SHAREDLIB_SPEC,
|
||||
SHARED_LIB_SPEC_DECISION "extern GLOBAL_SHAREDLIB_SPEC double dInfinityValue;\n#define dInfinity dInfinityValue" UNDEF_SHAREDLIB_SPEC,
|
||||
SHARED_LIB_SPEC_DECISION "extern GLOBAL_SHAREDLIB_SPEC double dInfinityValue;\n#define dInfinity dInfinityValue" UNDEF_SHAREDLIB_SPEC};
|
||||
#endif
|
||||
|
||||
write_header_comment (file,"select the base floating point type");
|
||||
#ifdef dSINGLE
|
||||
fprintf (file,"#define dSINGLE 1\n\n");
|
||||
type = "float";
|
||||
suffix = "f";
|
||||
#else
|
||||
fprintf (file,"#define dDOUBLE 1\n\n");
|
||||
type = "double";
|
||||
suffix = "";
|
||||
#endif
|
||||
|
||||
/* infinity */
|
||||
write_header_comment (file,"the floating point infinity");
|
||||
|
||||
/* try the different infinity constants until one works */
|
||||
for (i=0; i < sizeof(inf)/sizeof(char*); i++) {
|
||||
f = xfopen ("ctest.c","wt");
|
||||
fprintf (f,
|
||||
"#include <stdio.h>\n"
|
||||
"#define SHARED_CONFIG_H_INCLUDED_FROM_DEFINING_FILE 1\n"
|
||||
SETUP_SHLIB_DEFS
|
||||
"%s\n"
|
||||
"%s\n"
|
||||
"%s\n"
|
||||
"int main() {\n"
|
||||
" if (dInfinity > 1e10%s && -dInfinity < -1e10%s &&\n"
|
||||
" -dInfinity < dInfinity) {\n"
|
||||
" FILE *f = fopen (\"data\",\"wt\");\n"
|
||||
" fprintf (f,\"foo\\n\");\n"
|
||||
" fclose (f);\n"
|
||||
" }\n"
|
||||
" return 0;\n"
|
||||
"}\n"
|
||||
,inc[i],inf[i],decl[i],suffix,suffix);
|
||||
fclose (f);
|
||||
delete_file ("data");
|
||||
compile ("ctest.exe","ctest.c");
|
||||
run ("ctest.exe");
|
||||
if (file_exists ("data")) {
|
||||
fprintf (file,"#define DINFINITY_DECL %s\n",decl[i]);
|
||||
fprintf (file,"%s\n\n",inf[i]);
|
||||
delete_file ("ctest.c");
|
||||
delete_file ("ctest.exe");
|
||||
delete_file ("data");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
fatal_error ("can't determine dInfinity constant");
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
|
||||
int main (int argc, char **argv)
|
||||
{
|
||||
FILE *file;
|
||||
|
||||
if (argc < 4 || argc > 5)
|
||||
fatal_error ("configurator expects 3 or 4 arguments");
|
||||
compile_cmd_line = argv[2];
|
||||
delete_cmd_line = argv[3];
|
||||
if (argc >= 5) run_prefix = argv[4];
|
||||
|
||||
/* check some defines we should have been compiled with */
|
||||
#if !defined(dSINGLE) && !defined(dDOUBLE)
|
||||
fatal_error ("you must set PRECISION to either SINGLE or DOUBLE");
|
||||
#endif
|
||||
|
||||
file = xfopen (argv[1],"wt");
|
||||
fprintf (file,config_h_part1);
|
||||
get_all_standard_headers (file);
|
||||
fprintf (file,config_h_part2);
|
||||
check_if_this_is_a_pentium (file);
|
||||
get_ODE_integer_typedefs (file);
|
||||
get_ODE_float_stuff (file);
|
||||
fprintf (file,config_h_footer);
|
||||
fclose (file);
|
||||
|
||||
printf ("\n*** configurator succeeded ***\n\n");
|
||||
return 0;
|
||||
}
|
||||
18
extern/ode/dist/include/ode/README
vendored
18
extern/ode/dist/include/ode/README
vendored
@@ -1,18 +0,0 @@
|
||||
|
||||
this is the public C interface to the ODE library.
|
||||
|
||||
all these files should be includable from C, i.e. they should not use any
|
||||
C++ features. everything should be protected with
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
...
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
the only exception is the odecpp.h file, which defines a C++ wrapper for
|
||||
the C interface. remember to keep this in sync!
|
||||
307
extern/ode/dist/include/ode/common.h
vendored
307
extern/ode/dist/include/ode/common.h
vendored
@@ -1,307 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef _ODE_COMMON_H_
|
||||
#define _ODE_COMMON_H_
|
||||
|
||||
#include <ode/config.h>
|
||||
#include <ode/error.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
/* configuration stuff */
|
||||
|
||||
/* the efficient alignment. most platforms align data structures to some
|
||||
* number of bytes, but this is not always the most efficient alignment.
|
||||
* for example, many x86 compilers align to 4 bytes, but on a pentium it
|
||||
* is important to align doubles to 8 byte boundaries (for speed), and
|
||||
* the 4 floats in a SIMD register to 16 byte boundaries. many other
|
||||
* platforms have similar behavior. setting a larger alignment can waste
|
||||
* a (very) small amount of memory. NOTE: this number must be a power of
|
||||
* two. this is set to 16 by default.
|
||||
*/
|
||||
#define EFFICIENT_ALIGNMENT 16
|
||||
|
||||
|
||||
/* constants */
|
||||
|
||||
/* pi and 1/sqrt(2) are defined here if necessary because they don't get
|
||||
* defined in <math.h> on some platforms (like MS-Windows)
|
||||
*/
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI REAL(3.1415926535897932384626433832795029)
|
||||
#endif
|
||||
#ifndef M_SQRT1_2
|
||||
#define M_SQRT1_2 REAL(0.7071067811865475244008443621048490)
|
||||
#endif
|
||||
|
||||
|
||||
/* debugging:
|
||||
* IASSERT is an internal assertion, i.e. a consistency check. if it fails
|
||||
* we want to know where.
|
||||
* UASSERT is a user assertion, i.e. if it fails a nice error message
|
||||
* should be printed for the user.
|
||||
* AASSERT is an arguments assertion, i.e. if it fails "bad argument(s)"
|
||||
* is printed.
|
||||
* DEBUGMSG just prints out a message
|
||||
*/
|
||||
|
||||
#ifndef dNODEBUG
|
||||
#ifdef __GNUC__
|
||||
#define dIASSERT(a) if (!(a)) dDebug (d_ERR_IASSERT, \
|
||||
"assertion \"" #a "\" failed in %s() [%s]",__FUNCTION__,__FILE__);
|
||||
#define dUASSERT(a,msg) if (!(a)) dDebug (d_ERR_UASSERT, \
|
||||
msg " in %s()", __FUNCTION__);
|
||||
#define dDEBUGMSG(msg) dMessage (d_ERR_UASSERT, \
|
||||
msg " in %s()", __FUNCTION__);
|
||||
#else
|
||||
#define dIASSERT(a) if (!(a)) dDebug (d_ERR_IASSERT, \
|
||||
"assertion \"" #a "\" failed in %s:%d",__FILE__,__LINE__);
|
||||
#define dUASSERT(a,msg) if (!(a)) dDebug (d_ERR_UASSERT, \
|
||||
msg " (%s:%d)", __FILE__,__LINE__);
|
||||
#define dDEBUGMSG(msg) dMessage (d_ERR_UASSERT, \
|
||||
msg " (%s:%d)", __FILE__,__LINE__);
|
||||
#endif
|
||||
#else
|
||||
#define dIASSERT(a) ;
|
||||
#define dUASSERT(a,msg) ;
|
||||
#define dDEBUGMSG(msg) ;
|
||||
#endif
|
||||
#define dAASSERT(a) dUASSERT(a,"Bad argument(s)")
|
||||
|
||||
/* floating point data type, vector, matrix and quaternion types */
|
||||
|
||||
#if defined(dSINGLE)
|
||||
typedef float dReal;
|
||||
#elif defined(dDOUBLE)
|
||||
typedef double dReal;
|
||||
#else
|
||||
#error You must #define dSINGLE or dDOUBLE
|
||||
#endif
|
||||
|
||||
|
||||
/* round an integer up to a multiple of 4, except that 0 and 1 are unmodified
|
||||
* (used to compute matrix leading dimensions)
|
||||
*/
|
||||
#define dPAD(a) (((a) > 1) ? ((((a)-1)|3)+1) : (a))
|
||||
|
||||
/* these types are mainly just used in headers */
|
||||
typedef dReal dVector3[4];
|
||||
typedef dReal dVector4[4];
|
||||
typedef dReal dMatrix3[4*3];
|
||||
typedef dReal dMatrix4[4*4];
|
||||
typedef dReal dMatrix6[8*6];
|
||||
typedef dReal dQuaternion[4];
|
||||
|
||||
|
||||
/* precision dependent scalar math functions */
|
||||
|
||||
#if defined(dSINGLE)
|
||||
|
||||
#define REAL(x) (x ## f) /* form a constant */
|
||||
#define dRecip(x) ((float)(1.0f/(x))) /* reciprocal */
|
||||
#define dSqrt(x) ((float)sqrt(x)) /* square root */
|
||||
#define dRecipSqrt(x) ((float)(1.0f/sqrt(x))) /* reciprocal square root */
|
||||
#define dSin(x) ((float)sin(x)) /* sine */
|
||||
#define dCos(x) ((float)cos(x)) /* cosine */
|
||||
#define dFabs(x) ((float)fabs(x)) /* absolute value */
|
||||
#define dAtan2(y,x) ((float)atan2((y),(x))) /* arc tangent with 2 args */
|
||||
|
||||
#elif defined(dDOUBLE)
|
||||
|
||||
#define REAL(x) (x)
|
||||
#define dRecip(x) (1.0/(x))
|
||||
#define dSqrt(x) sqrt(x)
|
||||
#define dRecipSqrt(x) (1.0/sqrt(x))
|
||||
#define dSin(x) sin(x)
|
||||
#define dCos(x) cos(x)
|
||||
#define dFabs(x) fabs(x)
|
||||
#define dAtan2(y,x) atan2((y),(x))
|
||||
|
||||
#else
|
||||
#error You must #define dSINGLE or dDOUBLE
|
||||
#endif
|
||||
|
||||
|
||||
/* utility */
|
||||
|
||||
|
||||
/* round something up to be a multiple of the EFFICIENT_ALIGNMENT */
|
||||
|
||||
#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1)
|
||||
|
||||
|
||||
/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste
|
||||
* up to 15 bytes per allocation, depending on what alloca() returns.
|
||||
*/
|
||||
|
||||
#define dALLOCA16(n) \
|
||||
((char*)dEFFICIENT_SIZE(((int)(alloca((n)+(EFFICIENT_ALIGNMENT-1))))))
|
||||
|
||||
|
||||
/* internal object types (all prefixed with `dx') */
|
||||
|
||||
struct dxWorld; /* dynamics world */
|
||||
struct dxSpace; /* collision space */
|
||||
struct dxBody; /* rigid body (dynamics object) */
|
||||
struct dxGeom; /* geometry (collision object) */
|
||||
struct dxJoint;
|
||||
struct dxJointNode;
|
||||
struct dxJointGroup;
|
||||
|
||||
typedef struct dxWorld *dWorldID;
|
||||
typedef struct dxSpace *dSpaceID;
|
||||
typedef struct dxBody *dBodyID;
|
||||
typedef struct dxGeom *dGeomID;
|
||||
typedef struct dxJoint *dJointID;
|
||||
typedef struct dxJointGroup *dJointGroupID;
|
||||
|
||||
|
||||
/* error numbers */
|
||||
|
||||
enum {
|
||||
d_ERR_UNKNOWN = 0, /* unknown error */
|
||||
d_ERR_IASSERT, /* internal assertion failed */
|
||||
d_ERR_UASSERT, /* user assertion failed */
|
||||
d_ERR_LCP /* user assertion failed */
|
||||
};
|
||||
|
||||
|
||||
/* joint type numbers */
|
||||
|
||||
enum {
|
||||
dJointTypeNone = 0, /* or "unknown" */
|
||||
dJointTypeBall,
|
||||
dJointTypeHinge,
|
||||
dJointTypeSlider,
|
||||
dJointTypeContact,
|
||||
dJointTypeUniversal,
|
||||
dJointTypeHinge2,
|
||||
dJointTypeFixed,
|
||||
dJointTypeNull,
|
||||
dJointTypeAMotor
|
||||
};
|
||||
|
||||
|
||||
/* an alternative way of setting joint parameters, using joint parameter
|
||||
* structures and member constants. we don't actually do this yet.
|
||||
*/
|
||||
|
||||
/*
|
||||
typedef struct dLimot {
|
||||
int mode;
|
||||
dReal lostop, histop;
|
||||
dReal vel, fmax;
|
||||
dReal fudge_factor;
|
||||
dReal bounce, soft;
|
||||
dReal suspension_erp, suspension_cfm;
|
||||
} dLimot;
|
||||
|
||||
enum {
|
||||
dLimotLoStop = 0x0001,
|
||||
dLimotHiStop = 0x0002,
|
||||
dLimotVel = 0x0004,
|
||||
dLimotFMax = 0x0008,
|
||||
dLimotFudgeFactor = 0x0010,
|
||||
dLimotBounce = 0x0020,
|
||||
dLimotSoft = 0x0040
|
||||
};
|
||||
*/
|
||||
|
||||
|
||||
/* standard joint parameter names. why are these here? - because we don't want
|
||||
* to include all the joint function definitions in joint.cpp. hmmmm.
|
||||
* MSVC complains if we call D_ALL_PARAM_NAMES_X with a blank second argument,
|
||||
* which is why we have the D_ALL_PARAM_NAMES macro as well. please copy and
|
||||
* paste between these two.
|
||||
*/
|
||||
|
||||
#define D_ALL_PARAM_NAMES(start) \
|
||||
/* parameters for limits and motors */ \
|
||||
dParamLoStop = start, \
|
||||
dParamHiStop, \
|
||||
dParamVel, \
|
||||
dParamFMax, \
|
||||
dParamFudgeFactor, \
|
||||
dParamBounce, \
|
||||
dParamCFM, \
|
||||
dParamStopERP, \
|
||||
dParamStopCFM, \
|
||||
/* parameters for suspension */ \
|
||||
dParamSuspensionERP, \
|
||||
dParamSuspensionCFM,
|
||||
|
||||
#define D_ALL_PARAM_NAMES_X(start,x) \
|
||||
/* parameters for limits and motors */ \
|
||||
dParamLoStop ## x = start, \
|
||||
dParamHiStop ## x, \
|
||||
dParamVel ## x, \
|
||||
dParamFMax ## x, \
|
||||
dParamFudgeFactor ## x, \
|
||||
dParamBounce ## x, \
|
||||
dParamCFM ## x, \
|
||||
dParamStopERP ## x, \
|
||||
dParamStopCFM ## x, \
|
||||
/* parameters for suspension */ \
|
||||
dParamSuspensionERP ## x, \
|
||||
dParamSuspensionCFM ## x,
|
||||
|
||||
enum {
|
||||
D_ALL_PARAM_NAMES(0)
|
||||
D_ALL_PARAM_NAMES_X(0x100,2)
|
||||
D_ALL_PARAM_NAMES_X(0x200,3)
|
||||
|
||||
/* add a multiple of this constant to the basic parameter numbers to get
|
||||
* the parameters for the second, third etc axes.
|
||||
*/
|
||||
dParamGroup=0x100
|
||||
};
|
||||
|
||||
|
||||
/* angular motor mode numbers */
|
||||
|
||||
enum{
|
||||
dAMotorUser = 0,
|
||||
dAMotorEuler = 1
|
||||
};
|
||||
|
||||
|
||||
/* joint force feedback information */
|
||||
|
||||
typedef struct dJointFeedback {
|
||||
dVector3 f1; // force applied to body 1
|
||||
dVector3 t1; // torque applied to body 1
|
||||
dVector3 f2; // force applied to body 2
|
||||
dVector3 t2; // torque applied to body 2
|
||||
} dJointFeedback;
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
91
extern/ode/dist/include/ode/contact.h
vendored
91
extern/ode/dist/include/ode/contact.h
vendored
@@ -1,91 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef _ODE_CONTACT_H_
|
||||
#define _ODE_CONTACT_H_
|
||||
|
||||
#include <ode/common.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
enum {
|
||||
dContactMu2 = 0x001,
|
||||
dContactFDir1 = 0x002,
|
||||
dContactBounce = 0x004,
|
||||
dContactSoftERP = 0x008,
|
||||
dContactSoftCFM = 0x010,
|
||||
dContactMotion1 = 0x020,
|
||||
dContactMotion2 = 0x040,
|
||||
dContactSlip1 = 0x080,
|
||||
dContactSlip2 = 0x100,
|
||||
|
||||
dContactApprox0 = 0x0000,
|
||||
dContactApprox1_1 = 0x1000,
|
||||
dContactApprox1_2 = 0x2000,
|
||||
dContactApprox1 = 0x3000
|
||||
};
|
||||
|
||||
|
||||
typedef struct dSurfaceParameters {
|
||||
/* must always be defined */
|
||||
int mode;
|
||||
dReal mu;
|
||||
|
||||
/* only defined if the corresponding flag is set in mode */
|
||||
dReal mu2;
|
||||
dReal bounce;
|
||||
dReal bounce_vel;
|
||||
dReal soft_erp;
|
||||
dReal soft_cfm;
|
||||
dReal motion1,motion2;
|
||||
dReal slip1,slip2;
|
||||
} dSurfaceParameters;
|
||||
|
||||
|
||||
/* contact info set by collision functions */
|
||||
|
||||
typedef struct dContactGeom {
|
||||
dVector3 pos;
|
||||
dVector3 normal;
|
||||
dReal depth;
|
||||
dGeomID g1,g2;
|
||||
} dContactGeom;
|
||||
|
||||
|
||||
/* contact info used by contact joint */
|
||||
|
||||
typedef struct dContact {
|
||||
dSurfaceParameters surface;
|
||||
dContactGeom geom;
|
||||
dVector3 fdir1;
|
||||
} dContact;
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
64
extern/ode/dist/include/ode/error.h
vendored
64
extern/ode/dist/include/ode/error.h
vendored
@@ -1,64 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
/* this comes from the `reuse' library. copy any changes back to the source */
|
||||
|
||||
#ifndef _ODE_ERROR_H_
|
||||
#define _ODE_ERROR_H_
|
||||
|
||||
#include <ode/config.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* all user defined error functions have this type. error and debug functions
|
||||
* should not return.
|
||||
*/
|
||||
typedef void dMessageFunction (int errnum, const char *msg, va_list ap);
|
||||
|
||||
/* set a new error, debug or warning handler. if fn is 0, the default handlers
|
||||
* are used.
|
||||
*/
|
||||
void dSetErrorHandler (dMessageFunction *fn);
|
||||
void dSetDebugHandler (dMessageFunction *fn);
|
||||
void dSetMessageHandler (dMessageFunction *fn);
|
||||
|
||||
/* return the current error, debug or warning handler. if the return value is
|
||||
* 0, the default handlers are in place.
|
||||
*/
|
||||
dMessageFunction *dGetErrorHandler();
|
||||
dMessageFunction *dGetDebugHandler();
|
||||
dMessageFunction *dGetMessageHandler();
|
||||
|
||||
/* generate a fatal error, debug trap or a message. */
|
||||
void dError (int num, const char *msg, ...);
|
||||
void dDebug (int num, const char *msg, ...);
|
||||
void dMessage (int num, const char *msg, ...);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
153
extern/ode/dist/include/ode/geom.h
vendored
153
extern/ode/dist/include/ode/geom.h
vendored
@@ -1,153 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef _ODE_GEOM_H_
|
||||
#define _ODE_GEOM_H_
|
||||
|
||||
#include <ode/common.h>
|
||||
#include <ode/space.h>
|
||||
#include <ode/contact.h>
|
||||
|
||||
#if defined SHARED_GEOM_H_INCLUDED_FROM_DEFINING_FILE
|
||||
#define GLOBAL_SHAREDLIB_SPEC SHAREDLIBEXPORT
|
||||
#else
|
||||
#define GLOBAL_SHAREDLIB_SPEC SHAREDLIBIMPORT
|
||||
#endif
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* ************************************************************************ */
|
||||
/* utility functions */
|
||||
|
||||
void dClosestLineSegmentPoints (const dVector3 a1, const dVector3 a2,
|
||||
const dVector3 b1, const dVector3 b2,
|
||||
dVector3 cp1, dVector3 cp2);
|
||||
|
||||
int dBoxTouchesBox (const dVector3 _p1, const dMatrix3 R1,
|
||||
const dVector3 side1, const dVector3 _p2,
|
||||
const dMatrix3 R2, const dVector3 side2);
|
||||
|
||||
void dInfiniteAABB (dGeomID geom, dReal aabb[6]);
|
||||
void dCloseODE();
|
||||
|
||||
/* ************************************************************************ */
|
||||
/* standard classes */
|
||||
|
||||
/* class numbers */
|
||||
extern GLOBAL_SHAREDLIB_SPEC int dSphereClass;
|
||||
extern GLOBAL_SHAREDLIB_SPEC int dBoxClass;
|
||||
extern GLOBAL_SHAREDLIB_SPEC int dCCylinderClass;
|
||||
extern GLOBAL_SHAREDLIB_SPEC int dPlaneClass;
|
||||
extern GLOBAL_SHAREDLIB_SPEC int dGeomGroupClass;
|
||||
extern GLOBAL_SHAREDLIB_SPEC int dGeomTransformClass;
|
||||
|
||||
/* constructors */
|
||||
dGeomID dCreateSphere (dSpaceID space, dReal radius);
|
||||
dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz);
|
||||
dGeomID dCreatePlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d);
|
||||
dGeomID dCreateCCylinder (dSpaceID space, dReal radius, dReal length);
|
||||
dGeomID dCreateGeomGroup (dSpaceID space);
|
||||
|
||||
/* set geometry parameters */
|
||||
void dGeomSphereSetRadius (dGeomID sphere, dReal radius);
|
||||
void dGeomBoxSetLengths (dGeomID box, dReal lx, dReal ly, dReal lz);
|
||||
void dGeomPlaneSetParams (dGeomID plane, dReal a, dReal b, dReal c, dReal d);
|
||||
void dGeomCCylinderSetParams (dGeomID ccylinder, dReal radius, dReal length);
|
||||
|
||||
/* get geometry parameters */
|
||||
int dGeomGetClass (dGeomID);
|
||||
dReal dGeomSphereGetRadius (dGeomID sphere);
|
||||
void dGeomBoxGetLengths (dGeomID box, dVector3 result);
|
||||
void dGeomPlaneGetParams (dGeomID plane, dVector4 result);
|
||||
void dGeomCCylinderGetParams (dGeomID ccylinder,
|
||||
dReal *radius, dReal *length);
|
||||
|
||||
/* general functions */
|
||||
void dGeomSetData (dGeomID, void *);
|
||||
void *dGeomGetData (dGeomID);
|
||||
void dGeomSetBody (dGeomID, dBodyID);
|
||||
dBodyID dGeomGetBody (dGeomID);
|
||||
void dGeomSetPosition (dGeomID, dReal x, dReal y, dReal z);
|
||||
void dGeomSetRotation (dGeomID, const dMatrix3 R);
|
||||
const dReal * dGeomGetPosition (dGeomID);
|
||||
const dReal * dGeomGetRotation (dGeomID);
|
||||
void dGeomDestroy (dGeomID);
|
||||
void dGeomGetAABB (dGeomID, dReal aabb[6]);
|
||||
dReal *dGeomGetSpaceAABB (dGeomID);
|
||||
|
||||
/* ************************************************************************ */
|
||||
/* geometry group functions */
|
||||
|
||||
void dGeomGroupAdd (dGeomID group, dGeomID x);
|
||||
void dGeomGroupRemove (dGeomID group, dGeomID x);
|
||||
int dGeomGroupGetNumGeoms (dGeomID group);
|
||||
dGeomID dGeomGroupGetGeom (dGeomID group, int i);
|
||||
|
||||
/* ************************************************************************ */
|
||||
/* transformed geometry functions */
|
||||
|
||||
dGeomID dCreateGeomTransform (dSpaceID space);
|
||||
void dGeomTransformSetGeom (dGeomID g, dGeomID obj);
|
||||
dGeomID dGeomTransformGetGeom (dGeomID g);
|
||||
void dGeomTransformSetCleanup (dGeomID g, int mode);
|
||||
int dGeomTransformGetCleanup (dGeomID g);
|
||||
void dGeomTransformSetInfo (dGeomID g, int mode);
|
||||
int dGeomTransformGetInfo (dGeomID g);
|
||||
|
||||
/* ************************************************************************ */
|
||||
/* general collision */
|
||||
|
||||
int dCollide (dGeomID o1, dGeomID o2, int flags, dContactGeom *contact,
|
||||
int skip);
|
||||
|
||||
/* ************************************************************************ */
|
||||
/* custom classes */
|
||||
|
||||
typedef void dGetAABBFn (dGeomID, dReal aabb[6]);
|
||||
typedef int dColliderFn (dGeomID o1, dGeomID o2,
|
||||
int flags, dContactGeom *contact, int skip);
|
||||
typedef dColliderFn * dGetColliderFnFn (int num);
|
||||
typedef void dGeomDtorFn (dGeomID o);
|
||||
typedef int dAABBTestFn (dGeomID o1, dGeomID o2, dReal aabb[6]);
|
||||
|
||||
typedef struct dGeomClass {
|
||||
int bytes;
|
||||
dGetColliderFnFn *collider;
|
||||
dGetAABBFn *aabb;
|
||||
dAABBTestFn *aabb_test;
|
||||
dGeomDtorFn *dtor;
|
||||
} dGeomClass;
|
||||
|
||||
int dCreateGeomClass (const dGeomClass *classptr);
|
||||
void * dGeomGetClassData (dGeomID);
|
||||
dGeomID dCreateGeom (int classnum);
|
||||
|
||||
/* ************************************************************************ */
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
98
extern/ode/dist/include/ode/mass.h
vendored
98
extern/ode/dist/include/ode/mass.h
vendored
@@ -1,98 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef _ODE_MASS_H_
|
||||
#define _ODE_MASS_H_
|
||||
|
||||
#include <ode/common.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
struct dMass;
|
||||
typedef struct dMass dMass;
|
||||
|
||||
|
||||
void dMassSetZero (dMass *);
|
||||
|
||||
void dMassSetParameters (dMass *, dReal themass,
|
||||
dReal cgx, dReal cgy, dReal cgz,
|
||||
dReal I11, dReal I22, dReal I33,
|
||||
dReal I12, dReal I13, dReal I23);
|
||||
|
||||
void dMassSetSphere (dMass *, dReal density, dReal radius);
|
||||
|
||||
void dMassSetCappedCylinder (dMass *, dReal density, int direction,
|
||||
dReal a, dReal b);
|
||||
|
||||
void dMassSetBox (dMass *, dReal density,
|
||||
dReal lx, dReal ly, dReal lz);
|
||||
|
||||
void dMassAdjust (dMass *, dReal newmass);
|
||||
|
||||
void dMassTranslate (dMass *, dReal x, dReal y, dReal z);
|
||||
|
||||
void dMassRotate (dMass *, const dMatrix3 R);
|
||||
|
||||
void dMassAdd (dMass *a, const dMass *b);
|
||||
|
||||
|
||||
|
||||
struct dMass {
|
||||
dReal mass;
|
||||
dVector4 c;
|
||||
dMatrix3 I;
|
||||
|
||||
#ifdef __cplusplus
|
||||
dMass()
|
||||
{ dMassSetZero (this); }
|
||||
void setZero()
|
||||
{ dMassSetZero (this); }
|
||||
void setParameters (dReal themass, dReal cgx, dReal cgy, dReal cgz,
|
||||
dReal I11, dReal I22, dReal I33,
|
||||
dReal I12, dReal I13, dReal I23)
|
||||
{ dMassSetParameters (this,themass,cgx,cgy,cgz,I11,I22,I33,I12,I13,I23); }
|
||||
void setSphere (dReal density, dReal radius)
|
||||
{ dMassSetSphere (this,density,radius); }
|
||||
void setCappedCylinder (dReal density, int direction, dReal a, dReal b)
|
||||
{ dMassSetCappedCylinder (this,density,direction,a,b); }
|
||||
void setBox (dReal density, dReal lx, dReal ly, dReal lz)
|
||||
{ dMassSetBox (this,density,lx,ly,lz); }
|
||||
void adjust (dReal newmass)
|
||||
{ dMassAdjust (this,newmass); }
|
||||
void translate (dReal x, dReal y, dReal z)
|
||||
{ dMassTranslate (this,x,y,z); }
|
||||
void rotate (const dMatrix3 R)
|
||||
{ dMassRotate (this,R); }
|
||||
void add (const dMass *b)
|
||||
{ dMassAdd (this,b); }
|
||||
#endif
|
||||
};
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
195
extern/ode/dist/include/ode/matrix.h
vendored
195
extern/ode/dist/include/ode/matrix.h
vendored
@@ -1,195 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
/* optimized and unoptimized vector and matrix functions */
|
||||
|
||||
#ifndef _ODE_MATRIX_H_
|
||||
#define _ODE_MATRIX_H_
|
||||
|
||||
#include <ode/common.h>
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
/* set a vector/matrix of size n to all zeros, or to a specific value. */
|
||||
|
||||
void dSetZero (dReal *a, int n);
|
||||
void dSetValue (dReal *a, int n, dReal value);
|
||||
|
||||
|
||||
/* get the dot product of two n*1 vectors. if n <= 0 then
|
||||
* zero will be returned (in which case a and b need not be valid).
|
||||
*/
|
||||
|
||||
dReal dDot (const dReal *a, const dReal *b, int n);
|
||||
|
||||
|
||||
/* get the dot products of (a0,b), (a1,b), etc and return them in outsum.
|
||||
* all vectors are n*1. if n <= 0 then zeroes will be returned (in which case
|
||||
* the input vectors need not be valid). this function is somewhat faster
|
||||
* than calling dDot() for all of the combinations separately.
|
||||
*/
|
||||
|
||||
/* NOT INCLUDED in the library for now.
|
||||
void dMultidot2 (const dReal *a0, const dReal *a1,
|
||||
const dReal *b, dReal *outsum, int n);
|
||||
*/
|
||||
|
||||
|
||||
/* matrix multiplication. all matrices are stored in standard row format.
|
||||
* the digit refers to the argument that is transposed:
|
||||
* 0: A = B * C (sizes: A:p*r B:p*q C:q*r)
|
||||
* 1: A = B' * C (sizes: A:p*r B:q*p C:q*r)
|
||||
* 2: A = B * C' (sizes: A:p*r B:p*q C:r*q)
|
||||
* case 1,2 are equivalent to saying that the operation is A=B*C but
|
||||
* B or C are stored in standard column format.
|
||||
*/
|
||||
|
||||
void dMultiply0 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);
|
||||
void dMultiply1 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);
|
||||
void dMultiply2 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);
|
||||
|
||||
|
||||
/* do an in-place cholesky decomposition on the lower triangle of the n*n
|
||||
* symmetric matrix A (which is stored by rows). the resulting lower triangle
|
||||
* will be such that L*L'=A. return 1 on success and 0 on failure (on failure
|
||||
* the matrix is not positive definite).
|
||||
*/
|
||||
|
||||
int dFactorCholesky (dReal *A, int n);
|
||||
|
||||
|
||||
/* solve for x: L*L'*x = b, and put the result back into x.
|
||||
* L is size n*n, b is size n*1. only the lower triangle of L is considered.
|
||||
*/
|
||||
|
||||
void dSolveCholesky (const dReal *L, dReal *b, int n);
|
||||
|
||||
|
||||
/* compute the inverse of the n*n positive definite matrix A and put it in
|
||||
* Ainv. this is not especially fast. this returns 1 on success (A was
|
||||
* positive definite) or 0 on failure (not PD).
|
||||
*/
|
||||
|
||||
int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n);
|
||||
|
||||
|
||||
/* check whether an n*n matrix A is positive definite, return 1/0 (yes/no).
|
||||
* positive definite means that x'*A*x > 0 for any x. this performs a
|
||||
* cholesky decomposition of A. if the decomposition fails then the matrix
|
||||
* is not positive definite. A is stored by rows. A is not altered.
|
||||
*/
|
||||
|
||||
int dIsPositiveDefinite (const dReal *A, int n);
|
||||
|
||||
|
||||
/* factorize a matrix A into L*D*L', where L is lower triangular with ones on
|
||||
* the diagonal, and D is diagonal.
|
||||
* A is an n*n matrix stored by rows, with a leading dimension of n rounded
|
||||
* up to 4. L is written into the strict lower triangle of A (the ones are not
|
||||
* written) and the reciprocal of the diagonal elements of D are written into
|
||||
* d.
|
||||
*/
|
||||
void dFactorLDLT (dReal *A, dReal *d, int n, int nskip);
|
||||
|
||||
|
||||
/* solve L*x=b, where L is n*n lower triangular with ones on the diagonal,
|
||||
* and x,b are n*1. b is overwritten with x.
|
||||
* the leading dimension of L is `nskip'.
|
||||
*/
|
||||
void dSolveL1 (const dReal *L, dReal *b, int n, int nskip);
|
||||
|
||||
|
||||
/* solve L'*x=b, where L is n*n lower triangular with ones on the diagonal,
|
||||
* and x,b are n*1. b is overwritten with x.
|
||||
* the leading dimension of L is `nskip'.
|
||||
*/
|
||||
void dSolveL1T (const dReal *L, dReal *b, int n, int nskip);
|
||||
|
||||
|
||||
/* in matlab syntax: a(1:n) = a(1:n) .* d(1:n) */
|
||||
|
||||
void dVectorScale (dReal *a, const dReal *d, int n);
|
||||
|
||||
|
||||
/* given `L', a n*n lower triangular matrix with ones on the diagonal,
|
||||
* and `d', a n*1 vector of the reciprocal diagonal elements of an n*n matrix
|
||||
* D, solve L*D*L'*x=b where x,b are n*1. x overwrites b.
|
||||
* the leading dimension of L is `nskip'.
|
||||
*/
|
||||
|
||||
void dSolveLDLT (const dReal *L, const dReal *d, dReal *b, int n, int nskip);
|
||||
|
||||
|
||||
/* given an L*D*L' factorization of an n*n matrix A, return the updated
|
||||
* factorization L2*D2*L2' of A plus the following "top left" matrix:
|
||||
*
|
||||
* [ b a' ] <-- b is a[0]
|
||||
* [ a 0 ] <-- a is a[1..n-1]
|
||||
*
|
||||
* - L has size n*n, its leading dimension is nskip. L is lower triangular
|
||||
* with ones on the diagonal. only the lower triangle of L is referenced.
|
||||
* - d has size n. d contains the reciprocal diagonal elements of D.
|
||||
* - a has size n.
|
||||
* the result is written into L, except that the left column of L and d[0]
|
||||
* are not actually modified. see ldltaddTL.m for further comments.
|
||||
*/
|
||||
void dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip);
|
||||
|
||||
|
||||
/* given an L*D*L' factorization of a permuted matrix A, produce a new
|
||||
* factorization for row and column `r' removed.
|
||||
* - A has size n1*n1, its leading dimension in nskip. A is symmetric and
|
||||
* positive definite. only the lower triangle of A is referenced.
|
||||
* A itself may actually be an array of row pointers.
|
||||
* - L has size n2*n2, its leading dimension in nskip. L is lower triangular
|
||||
* with ones on the diagonal. only the lower triangle of L is referenced.
|
||||
* - d has size n2. d contains the reciprocal diagonal elements of D.
|
||||
* - p is a permutation vector. it contains n2 indexes into A. each index
|
||||
* must be in the range 0..n1-1.
|
||||
* - r is the row/column of L to remove.
|
||||
* the new L will be written within the old L, i.e. will have the same leading
|
||||
* dimension. the last row and column of L, and the last element of d, are
|
||||
* undefined on exit.
|
||||
*
|
||||
* a fast O(n^2) algorithm is used. see ldltremove.m for further comments.
|
||||
*/
|
||||
void dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d,
|
||||
int n1, int n2, int r, int nskip);
|
||||
|
||||
|
||||
/* given an n*n matrix A (with leading dimension nskip), remove the r'th row
|
||||
* and column by moving elements. the new matrix will have the same leading
|
||||
* dimension. the last row and column of A are untouched on exit.
|
||||
*/
|
||||
void dRemoveRowCol (dReal *A, int n, int nskip, int r);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
64
extern/ode/dist/include/ode/memory.h
vendored
64
extern/ode/dist/include/ode/memory.h
vendored
@@ -1,64 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
/* this comes from the `reuse' library. copy any changes back to the source */
|
||||
|
||||
#ifndef _ODE_MEMORY_H_
|
||||
#define _ODE_MEMORY_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* function types to allocate and free memory */
|
||||
typedef void * dAllocFunction (int size);
|
||||
typedef void * dReallocFunction (void *ptr, int oldsize, int newsize);
|
||||
typedef void dFreeFunction (void *ptr, int size);
|
||||
|
||||
/* set new memory management functions. if fn is 0, the default handlers are
|
||||
* used. */
|
||||
void dSetAllocHandler (dAllocFunction *fn);
|
||||
void dSetReallocHandler (dReallocFunction *fn);
|
||||
void dSetFreeHandler (dFreeFunction *fn);
|
||||
|
||||
/* get current memory management functions */
|
||||
dAllocFunction *dGetAllocHandler ();
|
||||
dReallocFunction *dGetReallocHandler ();
|
||||
dFreeFunction *dGetFreeHandler ();
|
||||
|
||||
/* allocate and free memory. */
|
||||
void * dAlloc (int size);
|
||||
void * dRealloc (void *ptr, int oldsize, int newsize);
|
||||
void dFree (void *ptr, int size);
|
||||
|
||||
/* when alloc debugging is turned on, this indicates that the given block of
|
||||
* alloc()ed memory should not be reported as "still in use" when the program
|
||||
* exits.
|
||||
*/
|
||||
void dAllocDontReport (void *ptr);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
86
extern/ode/dist/include/ode/misc.h
vendored
86
extern/ode/dist/include/ode/misc.h
vendored
@@ -1,86 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
/* miscellaneous math functions. these are mostly useful for testing */
|
||||
|
||||
#ifndef _ODE_MISC_H_
|
||||
#define _ODE_MISC_H_
|
||||
|
||||
#include <ode/common.h>
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
/* return 1 if the random number generator is working. */
|
||||
int dTestRand();
|
||||
|
||||
/* return next 32 bit random number. this uses a not-very-random linear
|
||||
* congruential method.
|
||||
*/
|
||||
unsigned long dRand();
|
||||
|
||||
/* get and set the current random number seed. */
|
||||
unsigned long dRandGetSeed();
|
||||
void dRandSetSeed (unsigned long s);
|
||||
|
||||
/* return a random integer between 0..n-1. the distribution will get worse
|
||||
* as n approaches 2^32.
|
||||
*/
|
||||
int dRandInt (int n);
|
||||
|
||||
/* return a random real number between 0..1 */
|
||||
dReal dRandReal();
|
||||
|
||||
/* print out a matrix */
|
||||
#ifdef __cplusplus
|
||||
void dPrintMatrix (dReal *A, int n, int m, char *fmt = "%10.4f ",
|
||||
FILE *f=stdout);
|
||||
#else
|
||||
void dPrintMatrix (dReal *A, int n, int m, char *fmt, FILE *f);
|
||||
#endif
|
||||
|
||||
/* make a random vector with entries between +/- range. A has n elements. */
|
||||
void dMakeRandomVector (dReal *A, int n, dReal range);
|
||||
|
||||
/* make a random matrix with entries between +/- range. A has size n*m. */
|
||||
void dMakeRandomMatrix (dReal *A, int n, int m, dReal range);
|
||||
|
||||
/* clear the upper triangle of a square matrix */
|
||||
void dClearUpperTriangle (dReal *A, int n);
|
||||
|
||||
/* return the maximum element difference between the two n*m matrices */
|
||||
dReal dMaxDifference (const dReal *A, const dReal *B, int n, int m);
|
||||
|
||||
/* return the maximum element difference between the lower triangle of two
|
||||
* n*n matrices */
|
||||
dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
202
extern/ode/dist/include/ode/objects.h
vendored
202
extern/ode/dist/include/ode/objects.h
vendored
@@ -1,202 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef _ODE_OBJECTS_H_
|
||||
#define _ODE_OBJECTS_H_
|
||||
|
||||
#include <ode/common.h>
|
||||
#include <ode/mass.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* world */
|
||||
|
||||
dWorldID dWorldCreate();
|
||||
void dWorldDestroy (dWorldID);
|
||||
|
||||
void dWorldSetGravity (dWorldID, dReal x, dReal y, dReal z);
|
||||
void dWorldGetGravity (dWorldID, dVector3 gravity);
|
||||
void dWorldSetERP (dWorldID, dReal erp);
|
||||
dReal dWorldGetERP (dWorldID);
|
||||
void dWorldSetCFM (dWorldID, dReal cfm);
|
||||
dReal dWorldGetCFM (dWorldID);
|
||||
void dWorldStep (dWorldID, dReal stepsize);
|
||||
void dWorldImpulseToForce (dWorldID, dReal stepsize,
|
||||
dReal ix, dReal iy, dReal iz, dVector3 force);
|
||||
|
||||
/* bodies */
|
||||
|
||||
dBodyID dBodyCreate (dWorldID);
|
||||
void dBodyDestroy (dBodyID);
|
||||
|
||||
void dBodySetData (dBodyID, void *data);
|
||||
void *dBodyGetData (dBodyID);
|
||||
|
||||
void dBodySetPosition (dBodyID, dReal x, dReal y, dReal z);
|
||||
void dBodySetRotation (dBodyID, const dMatrix3 R);
|
||||
void dBodySetQuaternion (dBodyID, const dQuaternion q);
|
||||
void dBodySetLinearVel (dBodyID, dReal x, dReal y, dReal z);
|
||||
void dBodySetAngularVel (dBodyID, dReal x, dReal y, dReal z);
|
||||
const dReal * dBodyGetPosition (dBodyID);
|
||||
const dReal * dBodyGetRotation (dBodyID); /* ptr to 4x3 rot matrix */
|
||||
const dReal * dBodyGetQuaternion (dBodyID);
|
||||
const dReal * dBodyGetLinearVel (dBodyID);
|
||||
const dReal * dBodyGetAngularVel (dBodyID);
|
||||
|
||||
void dBodySetMass (dBodyID, const dMass *mass);
|
||||
void dBodyGetMass (dBodyID, dMass *mass);
|
||||
|
||||
void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz);
|
||||
void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz);
|
||||
void dBodyAddRelForce (dBodyID, dReal fx, dReal fy, dReal fz);
|
||||
void dBodyAddRelTorque (dBodyID, dReal fx, dReal fy, dReal fz);
|
||||
void dBodyAddForceAtPos (dBodyID, dReal fx, dReal fy, dReal fz,
|
||||
dReal px, dReal py, dReal pz);
|
||||
void dBodyAddForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz,
|
||||
dReal px, dReal py, dReal pz);
|
||||
void dBodyAddRelForceAtPos (dBodyID, dReal fx, dReal fy, dReal fz,
|
||||
dReal px, dReal py, dReal pz);
|
||||
void dBodyAddRelForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz,
|
||||
dReal px, dReal py, dReal pz);
|
||||
|
||||
const dReal * dBodyGetForce (dBodyID);
|
||||
const dReal * dBodyGetTorque (dBodyID);
|
||||
void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z);
|
||||
void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z);
|
||||
|
||||
void dBodyGetRelPointPos (dBodyID, dReal px, dReal py, dReal pz,
|
||||
dVector3 result);
|
||||
void dBodyGetRelPointVel (dBodyID, dReal px, dReal py, dReal pz,
|
||||
dVector3 result);
|
||||
void dBodyGetPointVel (dBodyID, dReal px, dReal py, dReal pz,
|
||||
dVector3 result);
|
||||
void dBodyGetPosRelPoint (dBodyID, dReal px, dReal py, dReal pz,
|
||||
dVector3 result);
|
||||
void dBodyVectorToWorld (dBodyID, dReal px, dReal py, dReal pz,
|
||||
dVector3 result);
|
||||
void dBodyVectorFromWorld (dBodyID, dReal px, dReal py, dReal pz,
|
||||
dVector3 result);
|
||||
|
||||
void dBodySetFiniteRotationMode (dBodyID, int mode);
|
||||
void dBodySetFiniteRotationAxis (dBodyID, dReal x, dReal y, dReal z);
|
||||
|
||||
int dBodyGetFiniteRotationMode (dBodyID);
|
||||
void dBodyGetFiniteRotationAxis (dBodyID, dVector3 result);
|
||||
|
||||
int dBodyGetNumJoints (dBodyID b);
|
||||
dJointID dBodyGetJoint (dBodyID, int index);
|
||||
|
||||
void dBodyEnable (dBodyID);
|
||||
void dBodyDisable (dBodyID);
|
||||
int dBodyIsEnabled (dBodyID);
|
||||
|
||||
void dBodySetGravityMode (dBodyID b, int mode);
|
||||
int dBodyGetGravityMode (dBodyID b);
|
||||
|
||||
|
||||
/* joints */
|
||||
|
||||
dJointID dJointCreateBall (dWorldID, dJointGroupID);
|
||||
dJointID dJointCreateHinge (dWorldID, dJointGroupID);
|
||||
dJointID dJointCreateSlider (dWorldID, dJointGroupID);
|
||||
dJointID dJointCreateContact (dWorldID, dJointGroupID, const dContact *);
|
||||
dJointID dJointCreateHinge2 (dWorldID, dJointGroupID);
|
||||
dJointID dJointCreateUniversal (dWorldID, dJointGroupID);
|
||||
dJointID dJointCreateFixed (dWorldID, dJointGroupID);
|
||||
dJointID dJointCreateNull (dWorldID, dJointGroupID);
|
||||
dJointID dJointCreateAMotor (dWorldID, dJointGroupID);
|
||||
|
||||
void dJointDestroy (dJointID);
|
||||
|
||||
dJointGroupID dJointGroupCreate (int max_size);
|
||||
void dJointGroupDestroy (dJointGroupID);
|
||||
void dJointGroupEmpty (dJointGroupID);
|
||||
|
||||
void dJointAttach (dJointID, dBodyID body1, dBodyID body2);
|
||||
void dJointSetData (dJointID, void *data);
|
||||
void *dJointGetData (dJointID);
|
||||
int dJointGetType (dJointID);
|
||||
dBodyID dJointGetBody (dJointID, int index);
|
||||
|
||||
void dJointSetFeedback (dJointID, dJointFeedback *);
|
||||
dJointFeedback *dJointGetFeedback (dJointID);
|
||||
|
||||
void dJointSetBallAnchor (dJointID, dReal x, dReal y, dReal z);
|
||||
void dJointSetHingeAnchor (dJointID, dReal x, dReal y, dReal z);
|
||||
void dJointSetHingeAxis (dJointID, dReal x, dReal y, dReal z);
|
||||
void dJointSetHingeParam (dJointID, int parameter, dReal value);
|
||||
void dJointSetSliderAxis (dJointID, dReal x, dReal y, dReal z);
|
||||
void dJointSetSliderParam (dJointID, int parameter, dReal value);
|
||||
void dJointSetHinge2Anchor (dJointID, dReal x, dReal y, dReal z);
|
||||
void dJointSetHinge2Axis1 (dJointID, dReal x, dReal y, dReal z);
|
||||
void dJointSetHinge2Axis2 (dJointID, dReal x, dReal y, dReal z);
|
||||
void dJointSetHinge2Param (dJointID, int parameter, dReal value);
|
||||
void dJointSetUniversalAnchor (dJointID, dReal x, dReal y, dReal z);
|
||||
void dJointSetUniversalAxis1 (dJointID, dReal x, dReal y, dReal z);
|
||||
void dJointSetUniversalAxis2 (dJointID, dReal x, dReal y, dReal z);
|
||||
void dJointSetFixed (dJointID);
|
||||
void dJointSetAMotorNumAxes (dJointID, int num);
|
||||
void dJointSetAMotorAxis (dJointID, int anum, int rel,
|
||||
dReal x, dReal y, dReal z);
|
||||
void dJointSetAMotorAngle (dJointID, int anum, dReal angle);
|
||||
void dJointSetAMotorParam (dJointID, int parameter, dReal value);
|
||||
void dJointSetAMotorMode (dJointID, int mode);
|
||||
|
||||
void dJointGetBallAnchor (dJointID, dVector3 result);
|
||||
void dJointGetHingeAnchor (dJointID, dVector3 result);
|
||||
void dJointGetHingeAxis (dJointID, dVector3 result);
|
||||
dReal dJointGetHingeParam (dJointID, int parameter);
|
||||
dReal dJointGetHingeAngle (dJointID);
|
||||
dReal dJointGetHingeAngleRate (dJointID);
|
||||
dReal dJointGetSliderPosition (dJointID);
|
||||
dReal dJointGetSliderPositionRate (dJointID);
|
||||
void dJointGetSliderAxis (dJointID, dVector3 result);
|
||||
dReal dJointGetSliderParam (dJointID, int parameter);
|
||||
void dJointGetHinge2Anchor (dJointID, dVector3 result);
|
||||
void dJointGetHinge2Axis1 (dJointID, dVector3 result);
|
||||
void dJointGetHinge2Axis2 (dJointID, dVector3 result);
|
||||
dReal dJointGetHinge2Param (dJointID, int parameter);
|
||||
dReal dJointGetHinge2Angle1 (dJointID);
|
||||
dReal dJointGetHinge2Angle1Rate (dJointID);
|
||||
dReal dJointGetHinge2Angle2Rate (dJointID);
|
||||
void dJointGetUniversalAnchor (dJointID, dVector3 result);
|
||||
void dJointGetUniversalAxis1 (dJointID, dVector3 result);
|
||||
void dJointGetUniversalAxis2 (dJointID, dVector3 result);
|
||||
int dJointGetAMotorNumAxes (dJointID);
|
||||
void dJointGetAMotorAxis (dJointID, int anum, dVector3 result);
|
||||
int dJointGetAMotorAxisRel (dJointID, int anum);
|
||||
dReal dJointGetAMotorAngle (dJointID, int anum);
|
||||
dReal dJointGetAMotorAngleRate (dJointID, int anum);
|
||||
dReal dJointGetAMotorParam (dJointID, int parameter);
|
||||
int dJointGetAMotorMode (dJointID);
|
||||
|
||||
int dAreConnected (dBodyID, dBodyID);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
45
extern/ode/dist/include/ode/ode.h
vendored
45
extern/ode/dist/include/ode/ode.h
vendored
@@ -1,45 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef _ODE_ODE_H_
|
||||
#define _ODE_ODE_H_
|
||||
|
||||
/* include *everything* here */
|
||||
|
||||
#include <ode/config.h>
|
||||
#include <ode/common.h>
|
||||
#include <ode/contact.h>
|
||||
#include <ode/error.h>
|
||||
#include <ode/memory.h>
|
||||
#include <ode/odemath.h>
|
||||
#include <ode/matrix.h>
|
||||
#include <ode/timer.h>
|
||||
#include <ode/rotation.h>
|
||||
#include <ode/mass.h>
|
||||
#include <ode/space.h>
|
||||
#include <ode/geom.h>
|
||||
#include <ode/misc.h>
|
||||
#include <ode/objects.h>
|
||||
#include <ode/odecpp.h>
|
||||
|
||||
#endif
|
||||
|
||||
797
extern/ode/dist/include/ode/odecpp.h
vendored
797
extern/ode/dist/include/ode/odecpp.h
vendored
@@ -1,797 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
// C++ interface for everything
|
||||
|
||||
|
||||
#ifndef _ODE_ODECPP_H_
|
||||
#define _ODE_ODECPP_H_
|
||||
#ifdef __cplusplus
|
||||
|
||||
#include <ode/error.h>
|
||||
|
||||
|
||||
class dWorld {
|
||||
dWorldID _id;
|
||||
|
||||
// intentionally undefined, don't use these
|
||||
dWorld (const dWorld &);
|
||||
void operator= (const dWorld &);
|
||||
|
||||
public:
|
||||
dWorld()
|
||||
{ _id = dWorldCreate(); }
|
||||
~dWorld()
|
||||
{ dWorldDestroy (_id); }
|
||||
|
||||
dWorldID id() const
|
||||
{ return _id; }
|
||||
operator dWorldID() const
|
||||
{ return _id; }
|
||||
|
||||
void setGravity (dReal x, dReal y, dReal z)
|
||||
{ dWorldSetGravity (_id,x,y,z); }
|
||||
void getGravity (dVector3 g) const
|
||||
{ dWorldGetGravity (_id,g); }
|
||||
|
||||
void setERP (dReal erp)
|
||||
{ dWorldSetERP(_id, erp); }
|
||||
dReal getERP() const
|
||||
{ return dWorldGetERP(_id); }
|
||||
|
||||
void setCFM (dReal cfm)
|
||||
{ dWorldSetCFM(_id, cfm); }
|
||||
dReal getCFM() const
|
||||
{ return dWorldGetCFM(_id); }
|
||||
|
||||
void step (dReal stepsize)
|
||||
{ dWorldStep (_id,stepsize); }
|
||||
|
||||
void impulseToForce (dReal stepsize, dReal ix, dReal iy, dReal iz,
|
||||
dVector3 force)
|
||||
{ dWorldImpulseToForce (_id,stepsize,ix,iy,iz,force); }
|
||||
};
|
||||
|
||||
|
||||
class dBody {
|
||||
dBodyID _id;
|
||||
|
||||
// intentionally undefined, don't use these
|
||||
dBody (const dBody &);
|
||||
void operator= (const dBody &);
|
||||
|
||||
public:
|
||||
dBody()
|
||||
{ _id = 0; }
|
||||
dBody (dWorldID world)
|
||||
{ _id = dBodyCreate (world); }
|
||||
~dBody()
|
||||
{ if (_id) dBodyDestroy (_id); }
|
||||
|
||||
void create (dWorldID world) {
|
||||
if (_id) dBodyDestroy (_id);
|
||||
_id = dBodyCreate (world);
|
||||
}
|
||||
|
||||
dBodyID id() const
|
||||
{ return _id; }
|
||||
operator dBodyID() const
|
||||
{ return _id; }
|
||||
|
||||
void setData (void *data)
|
||||
{ dBodySetData (_id,data); }
|
||||
void *getData() const
|
||||
{ return dBodyGetData (_id); }
|
||||
|
||||
void setPosition (dReal x, dReal y, dReal z)
|
||||
{ dBodySetPosition (_id,x,y,z); }
|
||||
void setRotation (const dMatrix3 R)
|
||||
{ dBodySetRotation (_id,R); }
|
||||
void setQuaternion (const dQuaternion q)
|
||||
{ dBodySetQuaternion (_id,q); }
|
||||
void setLinearVel (dReal x, dReal y, dReal z)
|
||||
{ dBodySetLinearVel (_id,x,y,z); }
|
||||
void setAngularVel (dReal x, dReal y, dReal z)
|
||||
{ dBodySetAngularVel (_id,x,y,z); }
|
||||
|
||||
const dReal * getPosition() const
|
||||
{ return dBodyGetPosition (_id); }
|
||||
const dReal * getRotation() const
|
||||
{ return dBodyGetRotation (_id); }
|
||||
const dReal * getQuaternion() const
|
||||
{ return dBodyGetQuaternion (_id); }
|
||||
const dReal * getLinearVel() const
|
||||
{ return dBodyGetLinearVel (_id); }
|
||||
const dReal * getAngularVel() const
|
||||
{ return dBodyGetAngularVel (_id); }
|
||||
|
||||
void setMass (const dMass *mass)
|
||||
{ dBodySetMass (_id,mass); }
|
||||
void getMass (dMass *mass) const
|
||||
{ dBodyGetMass (_id,mass); }
|
||||
|
||||
void addForce (dReal fx, dReal fy, dReal fz)
|
||||
{ dBodyAddForce (_id, fx, fy, fz); }
|
||||
void addTorque (dReal fx, dReal fy, dReal fz)
|
||||
{ dBodyAddTorque (_id, fx, fy, fz); }
|
||||
void addRelForce (dReal fx, dReal fy, dReal fz)
|
||||
{ dBodyAddRelForce (_id, fx, fy, fz); }
|
||||
void addRelTorque (dReal fx, dReal fy, dReal fz)
|
||||
{ dBodyAddRelTorque (_id, fx, fy, fz); }
|
||||
void addForceAtPos (dReal fx, dReal fy, dReal fz,
|
||||
dReal px, dReal py, dReal pz)
|
||||
{ dBodyAddForceAtPos (_id, fx, fy, fz, px, py, pz); }
|
||||
void addForceAtRelPos (dReal fx, dReal fy, dReal fz,
|
||||
dReal px, dReal py, dReal pz)
|
||||
{ dBodyAddForceAtRelPos (_id, fx, fy, fz, px, py, pz); }
|
||||
void addRelForceAtPos (dReal fx, dReal fy, dReal fz,
|
||||
dReal px, dReal py, dReal pz)
|
||||
{ dBodyAddRelForceAtPos (_id, fx, fy, fz, px, py, pz); }
|
||||
void addRelForceAtRelPos (dReal fx, dReal fy, dReal fz,
|
||||
dReal px, dReal py, dReal pz)
|
||||
{ dBodyAddRelForceAtRelPos (_id, fx, fy, fz, px, py, pz); }
|
||||
|
||||
const dReal * getForce() const
|
||||
{ return dBodyGetForce(_id); }
|
||||
const dReal * getTorque() const
|
||||
{ return dBodyGetTorque(_id); }
|
||||
void setForce (dReal x, dReal y, dReal z)
|
||||
{ dBodySetForce (_id,x,y,z); }
|
||||
void setTorque (dReal x, dReal y, dReal z)
|
||||
{ dBodySetTorque (_id,x,y,z); }
|
||||
|
||||
void enable()
|
||||
{ dBodyEnable (_id); }
|
||||
void disable()
|
||||
{ dBodyDisable (_id); }
|
||||
int isEnabled() const
|
||||
{ return dBodyIsEnabled (_id); }
|
||||
|
||||
void getRelPointPos (dReal px, dReal py, dReal pz, dVector3 result) const
|
||||
{ dBodyGetRelPointPos (_id, px, py, pz, result); }
|
||||
void getRelPointVel (dReal px, dReal py, dReal pz, dVector3 result) const
|
||||
{ dBodyGetRelPointVel (_id, px, py, pz, result); }
|
||||
void getPointVel (dReal px, dReal py, dReal pz, dVector3 result) const
|
||||
{ dBodyGetPointVel (_id,px,py,pz,result); }
|
||||
void getPosRelPoint (dReal px, dReal py, dReal pz, dVector3 result) const
|
||||
{ dBodyGetPosRelPoint (_id,px,py,pz,result); }
|
||||
void vectorToWorld (dReal px, dReal py, dReal pz, dVector3 result) const
|
||||
{ dBodyVectorToWorld (_id,px,py,pz,result); }
|
||||
void vectorFromWorld (dReal px, dReal py, dReal pz, dVector3 result) const
|
||||
{ dBodyVectorFromWorld (_id,px,py,pz,result); }
|
||||
|
||||
void setFiniteRotationMode (int mode)
|
||||
{ dBodySetFiniteRotationMode (_id, mode); }
|
||||
void setFiniteRotationAxis (dReal x, dReal y, dReal z)
|
||||
{ dBodySetFiniteRotationAxis (_id, x, y, z); }
|
||||
|
||||
int getFiniteRotationMode() const
|
||||
{ return dBodyGetFiniteRotationMode (_id); }
|
||||
void getFiniteRotationAxis (dVector3 result) const
|
||||
{ dBodyGetFiniteRotationAxis (_id, result); }
|
||||
|
||||
int getNumJoints() const
|
||||
{ return dBodyGetNumJoints (_id); }
|
||||
dJointID getJoint (int index) const
|
||||
{ return dBodyGetJoint (_id, index); }
|
||||
|
||||
void setGravityMode (int mode)
|
||||
{ dBodySetGravityMode (_id,mode); }
|
||||
int getGravityMode() const
|
||||
{ return dBodyGetGravityMode (_id); }
|
||||
|
||||
int isConnectedTo (dBodyID body) const
|
||||
{ return dAreConnected (_id, body); }
|
||||
};
|
||||
|
||||
|
||||
class dJointGroup {
|
||||
dJointGroupID _id;
|
||||
|
||||
// intentionally undefined, don't use these
|
||||
dJointGroup (const dJointGroup &);
|
||||
void operator= (const dJointGroup &);
|
||||
|
||||
public:
|
||||
dJointGroup (int dummy_arg=0)
|
||||
{ _id = dJointGroupCreate (0); }
|
||||
~dJointGroup()
|
||||
{ dJointGroupDestroy (_id); }
|
||||
void create (int dummy_arg=0) {
|
||||
if (_id) dJointGroupDestroy (_id);
|
||||
_id = dJointGroupCreate (0);
|
||||
}
|
||||
|
||||
dJointGroupID id() const
|
||||
{ return _id; }
|
||||
operator dJointGroupID() const
|
||||
{ return _id; }
|
||||
|
||||
void empty()
|
||||
{ dJointGroupEmpty (_id); }
|
||||
};
|
||||
|
||||
|
||||
class dJoint {
|
||||
private:
|
||||
// intentionally undefined, don't use these
|
||||
dJoint (const dJoint &) ;
|
||||
void operator= (const dJoint &);
|
||||
|
||||
protected:
|
||||
dJointID _id;
|
||||
|
||||
public:
|
||||
dJoint()
|
||||
{ _id = 0; }
|
||||
~dJoint()
|
||||
{ if (_id) dJointDestroy (_id); }
|
||||
|
||||
dJointID id() const
|
||||
{ return _id; }
|
||||
operator dJointID() const
|
||||
{ return _id; }
|
||||
|
||||
void attach (dBodyID body1, dBodyID body2)
|
||||
{ dJointAttach (_id, body1, body2); }
|
||||
|
||||
void setData (void *data)
|
||||
{ dJointSetData (_id, data); }
|
||||
void *getData (void *data) const
|
||||
{ return dJointGetData (_id); }
|
||||
|
||||
int getType() const
|
||||
{ return dJointGetType (_id); }
|
||||
|
||||
dBodyID getBody (int index) const
|
||||
{ return dJointGetBody (_id, index); }
|
||||
};
|
||||
|
||||
|
||||
class dBallJoint : public dJoint {
|
||||
private:
|
||||
// intentionally undefined, don't use these
|
||||
dBallJoint (const dBallJoint &);
|
||||
void operator= (const dBallJoint &);
|
||||
|
||||
public:
|
||||
dBallJoint() { }
|
||||
dBallJoint (dWorldID world, dJointGroupID group=0)
|
||||
{ _id = dJointCreateBall (world, group); }
|
||||
|
||||
void create (dWorldID world, dJointGroupID group=0) {
|
||||
if (_id) dJointDestroy (_id);
|
||||
_id = dJointCreateBall (world, group);
|
||||
}
|
||||
|
||||
void setAnchor (dReal x, dReal y, dReal z)
|
||||
{ dJointSetBallAnchor (_id, x, y, z); }
|
||||
void getAnchor (dVector3 result) const
|
||||
{ dJointGetBallAnchor (_id, result); }
|
||||
} ;
|
||||
|
||||
|
||||
class dHingeJoint : public dJoint {
|
||||
// intentionally undefined, don't use these
|
||||
dHingeJoint (const dHingeJoint &);
|
||||
void operator = (const dHingeJoint &);
|
||||
|
||||
public:
|
||||
dHingeJoint() { }
|
||||
dHingeJoint (dWorldID world, dJointGroupID group=0)
|
||||
{ _id = dJointCreateHinge (world, group); }
|
||||
|
||||
void create (dWorldID world, dJointGroupID group=0) {
|
||||
if (_id) dJointDestroy (_id);
|
||||
_id = dJointCreateHinge (world, group);
|
||||
}
|
||||
|
||||
void setAnchor (dReal x, dReal y, dReal z)
|
||||
{ dJointSetHingeAnchor (_id, x, y, z); }
|
||||
void getAnchor (dVector3 result) const
|
||||
{ dJointGetHingeAnchor (_id, result); }
|
||||
|
||||
void setAxis (dReal x, dReal y, dReal z)
|
||||
{ dJointSetHingeAxis (_id, x, y, z); }
|
||||
void getAxis (dVector3 result) const
|
||||
{ dJointGetHingeAxis (_id, result); }
|
||||
|
||||
dReal getAngle() const
|
||||
{ return dJointGetHingeAngle (_id); }
|
||||
dReal getAngleRate() const
|
||||
{ return dJointGetHingeAngleRate (_id); }
|
||||
|
||||
void setParam (int parameter, dReal value)
|
||||
{ dJointSetHingeParam (_id, parameter, value); }
|
||||
dReal getParam (int parameter) const
|
||||
{ return dJointGetHingeParam (_id, parameter); }
|
||||
};
|
||||
|
||||
|
||||
class dSliderJoint : public dJoint {
|
||||
// intentionally undefined, don't use these
|
||||
dSliderJoint (const dSliderJoint &);
|
||||
void operator = (const dSliderJoint &);
|
||||
|
||||
public:
|
||||
dSliderJoint() { }
|
||||
dSliderJoint (dWorldID world, dJointGroupID group=0)
|
||||
{ _id = dJointCreateSlider (world, group); }
|
||||
|
||||
void create (dWorldID world, dJointGroupID group=0) {
|
||||
if (_id) dJointDestroy (_id);
|
||||
_id = dJointCreateSlider (world, group);
|
||||
}
|
||||
|
||||
void setAxis (dReal x, dReal y, dReal z)
|
||||
{ dJointSetSliderAxis (_id, x, y, z); }
|
||||
void getAxis (dVector3 result) const
|
||||
{ dJointGetSliderAxis (_id, result); }
|
||||
|
||||
dReal getPosition() const
|
||||
{ return dJointGetSliderPosition (_id); }
|
||||
dReal getPositionRate() const
|
||||
{ return dJointGetSliderPositionRate (_id); }
|
||||
|
||||
void setParam (int parameter, dReal value)
|
||||
{ dJointSetSliderParam (_id, parameter, value); }
|
||||
dReal getParam (int parameter) const
|
||||
{ return dJointGetSliderParam (_id, parameter); }
|
||||
};
|
||||
|
||||
|
||||
class dUniversalJoint : public dJoint {
|
||||
// intentionally undefined, don't use these
|
||||
dUniversalJoint (const dUniversalJoint &);
|
||||
void operator = (const dUniversalJoint &);
|
||||
|
||||
public:
|
||||
dUniversalJoint() { }
|
||||
dUniversalJoint (dWorldID world, dJointGroupID group=0)
|
||||
{ _id = dJointCreateUniversal (world, group); }
|
||||
|
||||
void create (dWorldID world, dJointGroupID group=0) {
|
||||
if (_id) dJointDestroy (_id);
|
||||
_id = dJointCreateUniversal (world, group);
|
||||
}
|
||||
|
||||
void setAnchor (dReal x, dReal y, dReal z)
|
||||
{ dJointSetUniversalAnchor (_id, x, y, z); }
|
||||
void setAxis1 (dReal x, dReal y, dReal z)
|
||||
{ dJointSetUniversalAxis1 (_id, x, y, z); }
|
||||
void setAxis2 (dReal x, dReal y, dReal z)
|
||||
{ dJointSetUniversalAxis2 (_id, x, y, z); }
|
||||
|
||||
void getAnchor (dVector3 result) const
|
||||
{ dJointGetUniversalAnchor (_id, result); }
|
||||
void getAxis1 (dVector3 result) const
|
||||
{ dJointGetUniversalAxis1 (_id, result); }
|
||||
void getAxis2 (dVector3 result) const
|
||||
{ dJointGetUniversalAxis2 (_id, result); }
|
||||
};
|
||||
|
||||
|
||||
class dHinge2Joint : public dJoint {
|
||||
// intentionally undefined, don't use these
|
||||
dHinge2Joint (const dHinge2Joint &);
|
||||
void operator = (const dHinge2Joint &);
|
||||
|
||||
public:
|
||||
dHinge2Joint() { }
|
||||
dHinge2Joint (dWorldID world, dJointGroupID group=0)
|
||||
{ _id = dJointCreateHinge2 (world, group); }
|
||||
|
||||
void create (dWorldID world, dJointGroupID group=0) {
|
||||
if (_id) dJointDestroy (_id);
|
||||
_id = dJointCreateHinge2 (world, group);
|
||||
}
|
||||
|
||||
void setAnchor (dReal x, dReal y, dReal z)
|
||||
{ dJointSetHinge2Anchor (_id, x, y, z); }
|
||||
void setAxis1 (dReal x, dReal y, dReal z)
|
||||
{ dJointSetHinge2Axis1 (_id, x, y, z); }
|
||||
void setAxis2 (dReal x, dReal y, dReal z)
|
||||
{ dJointSetHinge2Axis2 (_id, x, y, z); }
|
||||
|
||||
void getAnchor (dVector3 result) const
|
||||
{ dJointGetHinge2Anchor (_id, result); }
|
||||
void getAxis1 (dVector3 result) const
|
||||
{ dJointGetHinge2Axis1 (_id, result); }
|
||||
void getAxis2 (dVector3 result) const
|
||||
{ dJointGetHinge2Axis2 (_id, result); }
|
||||
|
||||
dReal getAngle1() const
|
||||
{ return dJointGetHinge2Angle1 (_id); }
|
||||
dReal getAngle1Rate() const
|
||||
{ return dJointGetHinge2Angle1Rate (_id); }
|
||||
dReal getAngle2Rate() const
|
||||
{ return dJointGetHinge2Angle2Rate (_id); }
|
||||
|
||||
void setParam (int parameter, dReal value)
|
||||
{ dJointSetHinge2Param (_id, parameter, value); }
|
||||
dReal getParam (int parameter) const
|
||||
{ return dJointGetHinge2Param (_id, parameter); }
|
||||
};
|
||||
|
||||
|
||||
class dFixedJoint : public dJoint {
|
||||
// intentionally undefined, don't use these
|
||||
dFixedJoint (const dFixedJoint &);
|
||||
void operator = (const dFixedJoint &);
|
||||
|
||||
public:
|
||||
dFixedJoint() { }
|
||||
dFixedJoint (dWorldID world, dJointGroupID group=0)
|
||||
{ _id = dJointCreateFixed (world, group); }
|
||||
|
||||
void create (dWorldID world, dJointGroupID group=0) {
|
||||
if (_id) dJointDestroy (_id);
|
||||
_id = dJointCreateFixed (world, group);
|
||||
}
|
||||
|
||||
void set()
|
||||
{ dJointSetFixed (_id); }
|
||||
};
|
||||
|
||||
|
||||
class dContactJoint : public dJoint {
|
||||
// intentionally undefined, don't use these
|
||||
dContactJoint (const dContactJoint &);
|
||||
void operator = (const dContactJoint &);
|
||||
|
||||
public:
|
||||
dContactJoint() { }
|
||||
dContactJoint (dWorldID world, dJointGroupID group, dContact *contact)
|
||||
{ _id = dJointCreateContact (world, group, contact); }
|
||||
|
||||
void create (dWorldID world, dJointGroupID group, dContact *contact) {
|
||||
if (_id) dJointDestroy (_id);
|
||||
_id = dJointCreateContact (world, group, contact);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
class dNullJoint : public dJoint {
|
||||
// intentionally undefined, don't use these
|
||||
dNullJoint (const dNullJoint &);
|
||||
void operator = (const dNullJoint &);
|
||||
|
||||
public:
|
||||
dNullJoint() { }
|
||||
dNullJoint (dWorldID world, dJointGroupID group=0)
|
||||
{ _id = dJointCreateNull (world, group); }
|
||||
|
||||
void create (dWorldID world, dJointGroupID group=0) {
|
||||
if (_id) dJointDestroy (_id);
|
||||
_id = dJointCreateNull (world, group);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
class dAMotorJoint : public dJoint {
|
||||
// intentionally undefined, don't use these
|
||||
dAMotorJoint (const dAMotorJoint &);
|
||||
void operator = (const dAMotorJoint &);
|
||||
|
||||
public:
|
||||
dAMotorJoint() { }
|
||||
dAMotorJoint (dWorldID world, dJointGroupID group=0)
|
||||
{ _id = dJointCreateAMotor (world, group); }
|
||||
|
||||
void create (dWorldID world, dJointGroupID group=0) {
|
||||
if (_id) dJointDestroy (_id);
|
||||
_id = dJointCreateAMotor (world, group);
|
||||
}
|
||||
|
||||
void setMode (int mode)
|
||||
{ dJointSetAMotorMode (_id, mode); }
|
||||
int getMode() const
|
||||
{ return dJointGetAMotorMode (_id); }
|
||||
|
||||
void setNumAxes (int num)
|
||||
{ dJointSetAMotorNumAxes (_id, num); }
|
||||
int getNumAxes() const
|
||||
{ return dJointGetAMotorNumAxes (_id); }
|
||||
|
||||
void setAxis (int anum, int rel, dReal x, dReal y, dReal z)
|
||||
{ dJointSetAMotorAxis (_id, anum, rel, x, y, z); }
|
||||
void getAxis (int anum, dVector3 result) const
|
||||
{ dJointGetAMotorAxis (_id, anum, result); }
|
||||
int getAxisRel (int anum) const
|
||||
{ return dJointGetAMotorAxisRel (_id, anum); }
|
||||
|
||||
void setAngle (int anum, dReal angle)
|
||||
{ dJointSetAMotorAngle (_id, anum, angle); }
|
||||
dReal getAngle (int anum) const
|
||||
{ return dJointGetAMotorAngle (_id, anum); }
|
||||
dReal getAngleRate (int anum)
|
||||
{ return dJointGetAMotorAngleRate (_id,anum); }
|
||||
|
||||
void setParam (int parameter, dReal value)
|
||||
{ dJointSetAMotorParam (_id, parameter, value); }
|
||||
dReal getParam (int parameter) const
|
||||
{ return dJointGetAMotorParam (_id, parameter); }
|
||||
};
|
||||
|
||||
|
||||
class dGeom {
|
||||
// intentionally undefined, don't use these
|
||||
dGeom (dGeom &);
|
||||
void operator= (dGeom &);
|
||||
|
||||
protected:
|
||||
dGeomID _id;
|
||||
|
||||
public:
|
||||
dGeom()
|
||||
{ _id = 0; }
|
||||
~dGeom()
|
||||
{ if (_id) dGeomDestroy (_id); }
|
||||
|
||||
dGeomID id() const
|
||||
{ return _id; }
|
||||
operator dGeomID() const
|
||||
{ return _id; }
|
||||
|
||||
void destroy() {
|
||||
if (_id) dGeomDestroy (_id);
|
||||
_id = 0;
|
||||
}
|
||||
|
||||
int getClass() const
|
||||
{ return dGeomGetClass (_id); }
|
||||
|
||||
void setData (void *data)
|
||||
{ dGeomSetData (_id,data); }
|
||||
void *getData() const
|
||||
{ return dGeomGetData (_id); }
|
||||
|
||||
void setBody (dBodyID b)
|
||||
{ dGeomSetBody (_id,b); }
|
||||
dBodyID getBody() const
|
||||
{ return dGeomGetBody (_id); }
|
||||
|
||||
void setPosition (dReal x, dReal y, dReal z)
|
||||
{ dGeomSetPosition (_id,x,y,z); }
|
||||
const dReal * getPosition() const
|
||||
{ return dGeomGetPosition (_id); }
|
||||
|
||||
void setRotation (const dMatrix3 R)
|
||||
{ dGeomSetRotation (_id,R); }
|
||||
const dReal * getRotation() const
|
||||
{ return dGeomGetRotation (_id); }
|
||||
|
||||
void getAABB (dReal aabb[6]) const
|
||||
{ dGeomGetAABB (_id, aabb); }
|
||||
const dReal *getSpaceAABB() const
|
||||
{ return dGeomGetSpaceAABB (_id); }
|
||||
};
|
||||
|
||||
|
||||
class dSpace {
|
||||
// intentionally undefined, don't use these
|
||||
dSpace (dSpace &);
|
||||
void operator= (dSpace &);
|
||||
|
||||
protected:
|
||||
dSpaceID _id;
|
||||
|
||||
// the default constructor is protected so that you
|
||||
// can't instance this class. you must instance one
|
||||
// of its subclasses instead.
|
||||
dSpace () { _id = 0; }
|
||||
|
||||
public:
|
||||
~dSpace()
|
||||
{ dSpaceDestroy (_id); }
|
||||
|
||||
dSpaceID id() const
|
||||
{ return _id; }
|
||||
operator dSpaceID() const
|
||||
{ return _id; }
|
||||
|
||||
void add (dGeomID x)
|
||||
{ dSpaceAdd (_id, x); }
|
||||
void remove (dGeomID x)
|
||||
{ dSpaceRemove (_id, x); }
|
||||
int query (dGeomID x)
|
||||
{ return dSpaceQuery (_id,x); }
|
||||
|
||||
void collide (void *data, dNearCallback *callback)
|
||||
{ dSpaceCollide (_id,data,callback); }
|
||||
};
|
||||
|
||||
|
||||
class dSimpleSpace : public dSpace {
|
||||
// intentionally undefined, don't use these
|
||||
dSimpleSpace (dSimpleSpace &);
|
||||
void operator= (dSimpleSpace &);
|
||||
|
||||
public:
|
||||
dSimpleSpace ()
|
||||
{ _id = dSimpleSpaceCreate(); }
|
||||
};
|
||||
|
||||
|
||||
class dHashSpace : public dSpace {
|
||||
// intentionally undefined, don't use these
|
||||
dHashSpace (dHashSpace &);
|
||||
void operator= (dHashSpace &);
|
||||
|
||||
public:
|
||||
dHashSpace ()
|
||||
{ _id = dHashSpaceCreate(); }
|
||||
void setLevels (int minlevel, int maxlevel)
|
||||
{ dHashSpaceSetLevels (_id,minlevel,maxlevel); }
|
||||
};
|
||||
|
||||
|
||||
class dSphere : public dGeom {
|
||||
// intentionally undefined, don't use these
|
||||
dSphere (dSphere &);
|
||||
void operator= (dSphere &);
|
||||
|
||||
public:
|
||||
dSphere () { }
|
||||
dSphere (dSpaceID space, dReal radius)
|
||||
{ _id = dCreateSphere (space, radius); }
|
||||
|
||||
void create (dSpaceID space, dReal radius) {
|
||||
if (_id) dGeomDestroy (_id);
|
||||
_id = dCreateSphere (space, radius);
|
||||
}
|
||||
|
||||
void setRadius (dReal radius)
|
||||
{ dGeomSphereSetRadius (_id, radius); }
|
||||
dReal getRadius() const
|
||||
{ return dGeomSphereGetRadius (_id); }
|
||||
};
|
||||
|
||||
|
||||
class dBox : public dGeom {
|
||||
// intentionally undefined, don't use these
|
||||
dBox (dBox &);
|
||||
void operator= (dBox &);
|
||||
|
||||
public:
|
||||
dBox () { }
|
||||
dBox (dSpaceID space, dReal lx, dReal ly, dReal lz)
|
||||
{ _id = dCreateBox (space,lx,ly,lz); }
|
||||
|
||||
void create (dSpaceID space, dReal lx, dReal ly, dReal lz) {
|
||||
if (_id) dGeomDestroy (_id);
|
||||
_id = dCreateBox (space,lx,ly,lz);
|
||||
}
|
||||
|
||||
void setLengths (dReal lx, dReal ly, dReal lz)
|
||||
{ dGeomBoxSetLengths (_id, lx, ly, lz); }
|
||||
void getLengths (dVector3 result) const
|
||||
{ dGeomBoxGetLengths (_id,result); }
|
||||
};
|
||||
|
||||
|
||||
class dPlane : public dGeom {
|
||||
// intentionally undefined, don't use these
|
||||
dPlane (dPlane &);
|
||||
void operator= (dPlane &);
|
||||
|
||||
public:
|
||||
dPlane() { }
|
||||
dPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d)
|
||||
{ _id = dCreatePlane (space,a,b,c,d); }
|
||||
|
||||
void create (dSpaceID space, dReal a, dReal b, dReal c, dReal d) {
|
||||
if (_id) dGeomDestroy (_id);
|
||||
_id = dCreatePlane (space,a,b,c,d);
|
||||
}
|
||||
|
||||
void setParams (dReal a, dReal b, dReal c, dReal d)
|
||||
{ dGeomPlaneSetParams (_id, a, b, c, d); }
|
||||
void getParams (dVector4 result) const
|
||||
{ dGeomPlaneGetParams (_id,result); }
|
||||
};
|
||||
|
||||
|
||||
class dCCylinder : public dGeom {
|
||||
// intentionally undefined, don't use these
|
||||
dCCylinder (dCCylinder &);
|
||||
void operator= (dCCylinder &);
|
||||
|
||||
public:
|
||||
dCCylinder() { }
|
||||
dCCylinder (dSpaceID space, dReal radius, dReal length)
|
||||
{ _id = dCreateCCylinder (space,radius,length); }
|
||||
|
||||
void create (dSpaceID space, dReal radius, dReal length) {
|
||||
if (_id) dGeomDestroy (_id);
|
||||
_id = dCreateCCylinder (space,radius,length);
|
||||
}
|
||||
|
||||
void setParams (dReal radius, dReal length)
|
||||
{ dGeomCCylinderSetParams (_id, radius, length); }
|
||||
void getParams (dReal *radius, dReal *length) const
|
||||
{ dGeomCCylinderGetParams (_id,radius,length); }
|
||||
};
|
||||
|
||||
|
||||
class dGeomGroup : public dGeom {
|
||||
// intentionally undefined, don't use these
|
||||
dGeomGroup (dGeomGroup &);
|
||||
void operator= (dGeomGroup &);
|
||||
|
||||
public:
|
||||
dGeomGroup() { }
|
||||
dGeomGroup (dSpaceID space)
|
||||
{ _id = dCreateGeomGroup (space); }
|
||||
|
||||
void create (dSpaceID space=0) {
|
||||
if (_id) dGeomDestroy (_id);
|
||||
_id = dCreateGeomGroup (space);
|
||||
}
|
||||
|
||||
void add (dGeomID x)
|
||||
{ dGeomGroupAdd (_id, x); }
|
||||
void remove (dGeomID x)
|
||||
{ dGeomGroupRemove (_id, x); }
|
||||
|
||||
int getNumGeoms() const
|
||||
{ return dGeomGroupGetNumGeoms (_id); }
|
||||
dGeomID getGeom (int i) const
|
||||
{ return dGeomGroupGetGeom (_id, i); }
|
||||
};
|
||||
|
||||
|
||||
class dGeomTransform : public dGeom {
|
||||
// intentionally undefined, don't use these
|
||||
dGeomTransform (dGeomTransform &);
|
||||
void operator= (dGeomTransform &);
|
||||
|
||||
public:
|
||||
dGeomTransform() { }
|
||||
dGeomTransform (dSpaceID space)
|
||||
{ _id = dCreateGeomTransform (space); }
|
||||
|
||||
void create (dSpaceID space=0) {
|
||||
if (_id) dGeomDestroy (_id);
|
||||
_id = dCreateGeomTransform (space);
|
||||
}
|
||||
|
||||
void setGeom (dGeomID geom)
|
||||
{ dGeomTransformSetGeom (_id, geom); }
|
||||
dGeomID getGeom() const
|
||||
{ return dGeomTransformGetGeom (_id); }
|
||||
|
||||
void setCleanup (int mode)
|
||||
{ dGeomTransformSetCleanup (_id,mode); }
|
||||
int getCleanup (dGeomID g)
|
||||
{ return dGeomTransformGetCleanup (_id); }
|
||||
|
||||
void setInfo (int mode)
|
||||
{ dGeomTransformSetInfo (_id,mode); }
|
||||
int getInfo()
|
||||
{ return dGeomTransformGetInfo (_id); }
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
317
extern/ode/dist/include/ode/odecpp_old.h
vendored
317
extern/ode/dist/include/ode/odecpp_old.h
vendored
@@ -1,317 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
/* this is the old C++ interface, the new C++ interface is not quite
|
||||
* compatible with this. but this file is kept around in case you were
|
||||
* using the old interface.
|
||||
*/
|
||||
|
||||
#ifndef _ODE_ODECPP_H_
|
||||
#define _ODE_ODECPP_H_
|
||||
#ifdef __cplusplus
|
||||
|
||||
#include <ode/error.h>
|
||||
|
||||
|
||||
class dWorld {
|
||||
dWorldID _id;
|
||||
|
||||
dWorld (dWorld &) { dDebug (0,"bad"); }
|
||||
void operator= (dWorld &) { dDebug (0,"bad"); }
|
||||
|
||||
public:
|
||||
dWorld()
|
||||
{ _id = dWorldCreate(); }
|
||||
~dWorld()
|
||||
{ dWorldDestroy (_id); }
|
||||
dWorldID id()
|
||||
{ return _id; }
|
||||
|
||||
void setGravity (dReal x, dReal y, dReal z)
|
||||
{ dWorldSetGravity (_id,x,y,z); }
|
||||
void getGravity (dVector3 g)
|
||||
{ dWorldGetGravity (_id,g); }
|
||||
void step (dReal stepsize)
|
||||
{ dWorldStep (_id,stepsize); }
|
||||
};
|
||||
|
||||
|
||||
class dBody {
|
||||
dBodyID _id;
|
||||
|
||||
dBody (dBody &) { dDebug (0,"bad"); }
|
||||
void operator= (dBody &) { dDebug (0,"bad"); }
|
||||
|
||||
public:
|
||||
dBody()
|
||||
{ _id = 0; }
|
||||
dBody (dWorld &world)
|
||||
{ _id = dBodyCreate (world.id()); }
|
||||
~dBody()
|
||||
{ dBodyDestroy (_id); }
|
||||
void create (dWorld &world)
|
||||
{ if (_id) dBodyDestroy (_id); _id = dBodyCreate (world.id()); }
|
||||
dBodyID id()
|
||||
{ return _id; }
|
||||
|
||||
void setData (void *data)
|
||||
{ dBodySetData (_id,data); }
|
||||
void *getData()
|
||||
{ return dBodyGetData (_id); }
|
||||
|
||||
void setPosition (dReal x, dReal y, dReal z)
|
||||
{ dBodySetPosition (_id,x,y,z); }
|
||||
void setRotation (const dMatrix3 R)
|
||||
{ dBodySetRotation (_id,R); }
|
||||
void setQuaternion (const dQuaternion q)
|
||||
{ dBodySetQuaternion (_id,q); }
|
||||
void setLinearVel (dReal x, dReal y, dReal z)
|
||||
{ dBodySetLinearVel (_id,x,y,z); }
|
||||
void setAngularVel (dReal x, dReal y, dReal z)
|
||||
{ dBodySetAngularVel (_id,x,y,z); }
|
||||
|
||||
const dReal * getPosition()
|
||||
{ return dBodyGetPosition (_id); }
|
||||
const dReal * getRotation()
|
||||
{ return dBodyGetRotation (_id); }
|
||||
const dReal * getQuaternion()
|
||||
{ return dBodyGetQuaternion (_id); }
|
||||
const dReal * getLinearVel()
|
||||
{ return dBodyGetLinearVel (_id); }
|
||||
const dReal * getAngularVel()
|
||||
{ return dBodyGetAngularVel (_id); }
|
||||
|
||||
void setMass (const dMass *mass)
|
||||
{ dBodySetMass (_id,mass); }
|
||||
void getMass (dMass *mass)
|
||||
{ dBodyGetMass (_id,mass); }
|
||||
|
||||
void addForce (dReal fx, dReal fy, dReal fz)
|
||||
{ dBodyAddForce (_id, fx, fy, fz); }
|
||||
void addTorque (dReal fx, dReal fy, dReal fz)
|
||||
{ dBodyAddTorque (_id, fx, fy, fz); }
|
||||
void addRelForce (dReal fx, dReal fy, dReal fz)
|
||||
{ dBodyAddRelForce (_id, fx, fy, fz); }
|
||||
void addRelTorque (dReal fx, dReal fy, dReal fz)
|
||||
{ dBodyAddRelTorque (_id, fx, fy, fz); }
|
||||
void addForceAtPos (dReal fx, dReal fy, dReal fz,
|
||||
dReal px, dReal py, dReal pz)
|
||||
{ dBodyAddForceAtPos (_id, fx, fy, fz, px, py, pz); }
|
||||
void addRelForceAtPos (dReal fx, dReal fy, dReal fz,
|
||||
dReal px, dReal py, dReal pz)
|
||||
{ dBodyAddRelForceAtPos (_id, fx, fy, fz, px, py, pz); }
|
||||
void addRelForceAtRelPos (dReal fx, dReal fy, dReal fz,
|
||||
dReal px, dReal py, dReal pz)
|
||||
{ dBodyAddRelForceAtRelPos (_id, fx, fy, fz, px, py, pz); }
|
||||
|
||||
void getRelPointPos (dReal px, dReal py, dReal pz, dVector3 result)
|
||||
{ dBodyGetRelPointPos (_id, px, py, pz, result); }
|
||||
void getRelPointVel (dReal px, dReal py, dReal pz, dVector3 result)
|
||||
{ dBodyGetRelPointVel (_id, px, py, pz, result); }
|
||||
|
||||
int isConnectedTo (const dBody &b)
|
||||
{ return dAreConnected (_id,b._id); }
|
||||
};
|
||||
|
||||
|
||||
class dJointGroup {
|
||||
dJointGroupID _id;
|
||||
|
||||
dJointGroup (dJointGroup &) { dDebug (0,"bad"); }
|
||||
void operator= (dJointGroup &) { dDebug (0,"bad"); }
|
||||
|
||||
public:
|
||||
dJointGroup()
|
||||
{ _id = 0; }
|
||||
dJointGroup (int max_size)
|
||||
{ _id = dJointGroupCreate (max_size); }
|
||||
~dJointGroup()
|
||||
{ dJointGroupDestroy (_id); }
|
||||
void create (int max_size)
|
||||
{ if (_id) dJointGroupDestroy (_id); _id = dJointGroupCreate (max_size); }
|
||||
dJointGroupID id()
|
||||
{ return _id; }
|
||||
|
||||
void empty()
|
||||
{ dJointGroupEmpty (_id); }
|
||||
};
|
||||
|
||||
|
||||
class dJoint {
|
||||
dJointID _id;
|
||||
|
||||
dJoint (dJoint &) { dDebug (0,"bad"); }
|
||||
void operator= (dJoint &) { dDebug (0,"bad"); }
|
||||
|
||||
public:
|
||||
dJoint()
|
||||
{ _id = 0; }
|
||||
~dJoint()
|
||||
{ dJointDestroy (_id); }
|
||||
dJointID id()
|
||||
{ return _id; }
|
||||
|
||||
void createBall (dWorld &world, dJointGroup *group=0) {
|
||||
if (_id) dJointDestroy (_id);
|
||||
_id = dJointCreateBall (world.id(), group ? group->id() : 0);
|
||||
}
|
||||
void createHinge (dWorld &world, dJointGroup *group=0) {
|
||||
if (_id) dJointDestroy (_id);
|
||||
_id = dJointCreateHinge (world.id(), group ? group->id() : 0);
|
||||
}
|
||||
void createSlider (dWorld &world, dJointGroup *group=0) {
|
||||
if (_id) dJointDestroy (_id);
|
||||
_id = dJointCreateSlider (world.id(), group ? group->id() : 0);
|
||||
}
|
||||
void createContact (dWorld &world, dJointGroup *group, dContact *contact) {
|
||||
if (_id) dJointDestroy (_id);
|
||||
_id = dJointCreateContact (world.id(), group ? group->id() : 0, contact);
|
||||
}
|
||||
|
||||
void attach (dBody &body1, dBody &body2)
|
||||
{ dJointAttach (_id, body1.id(), body2.id()); }
|
||||
|
||||
void setBallAnchor (dReal x, dReal y, dReal z)
|
||||
{ dJointSetBallAnchor (_id, x, y, z); }
|
||||
void setHingeAnchor (dReal x, dReal y, dReal z)
|
||||
{ dJointSetHingeAnchor (_id, x, y, z); }
|
||||
|
||||
void setHingeAxis (dReal x, dReal y, dReal z)
|
||||
{ dJointSetHingeAxis (_id, x, y, z); }
|
||||
void setSliderAxis (dReal x, dReal y, dReal z)
|
||||
{ dJointSetSliderAxis (_id, x, y, z); }
|
||||
|
||||
void getBallAnchor (dVector3 result)
|
||||
{ dJointGetBallAnchor (_id, result); }
|
||||
void getHingeAnchor (dVector3 result)
|
||||
{ dJointGetHingeAnchor (_id, result); }
|
||||
|
||||
void getHingeAxis (dVector3 result)
|
||||
{ dJointGetHingeAxis (_id, result); }
|
||||
void getSliderAxis (dVector3 result)
|
||||
{ dJointGetSliderAxis (_id, result); }
|
||||
};
|
||||
|
||||
|
||||
class dSpace {
|
||||
dSpaceID _id;
|
||||
|
||||
dSpace (dSpace &) { dDebug (0,"bad"); }
|
||||
void operator= (dSpace &) { dDebug (0,"bad"); }
|
||||
|
||||
public:
|
||||
dSpace ()
|
||||
{ _id = dHashSpaceCreate(); }
|
||||
~dSpace()
|
||||
{ dSpaceDestroy (_id); }
|
||||
dSpaceID id()
|
||||
{ return _id; }
|
||||
void collide (void *data, dNearCallback *callback)
|
||||
{ dSpaceCollide (_id,data,callback); }
|
||||
};
|
||||
|
||||
|
||||
class dGeom {
|
||||
dGeomID _id;
|
||||
|
||||
dGeom (dGeom &) { dDebug (0,"bad"); }
|
||||
void operator= (dGeom &) { dDebug (0,"bad"); }
|
||||
|
||||
public:
|
||||
dGeom()
|
||||
{ _id = 0; }
|
||||
~dGeom()
|
||||
{ dGeomDestroy (_id); }
|
||||
dGeomID id()
|
||||
{ return _id; }
|
||||
|
||||
void createSphere (dSpace &space, dReal radius) {
|
||||
if (_id) dGeomDestroy (_id);
|
||||
_id = dCreateSphere (space.id(),radius);
|
||||
}
|
||||
|
||||
void createBox (dSpace &space, dReal lx, dReal ly, dReal lz) {
|
||||
if (_id) dGeomDestroy (_id);
|
||||
_id = dCreateBox (space.id(),lx,ly,lz);
|
||||
}
|
||||
|
||||
void createPlane (dSpace &space, dReal a, dReal b, dReal c, dReal d) {
|
||||
if (_id) dGeomDestroy (_id);
|
||||
_id = dCreatePlane (space.id(),a,b,c,d);
|
||||
}
|
||||
|
||||
void createCCylinder (dSpace &space, dReal radius, dReal length) {
|
||||
if (_id) dGeomDestroy (_id);
|
||||
_id = dCreateCCylinder (space.id(),radius,length);
|
||||
}
|
||||
|
||||
void destroy() {
|
||||
if (_id) dGeomDestroy (_id);
|
||||
_id = 0;
|
||||
}
|
||||
|
||||
int getClass()
|
||||
{ return dGeomGetClass (_id); }
|
||||
|
||||
dReal sphereGetRadius()
|
||||
{ return dGeomSphereGetRadius (_id); }
|
||||
|
||||
void boxGetLengths (dVector3 result)
|
||||
{ dGeomBoxGetLengths (_id,result); }
|
||||
|
||||
void planeGetParams (dVector4 result)
|
||||
{ dGeomPlaneGetParams (_id,result); }
|
||||
|
||||
void CCylinderGetParams (dReal *radius, dReal *length)
|
||||
{ dGeomCCylinderGetParams (_id,radius,length); }
|
||||
|
||||
void setData (void *data)
|
||||
{ dGeomSetData (_id,data); }
|
||||
|
||||
void *getData()
|
||||
{ return dGeomGetData (_id); }
|
||||
|
||||
void setBody (dBody &b)
|
||||
{ dGeomSetBody (_id,b.id()); }
|
||||
void setBody (dBodyID b)
|
||||
{ dGeomSetBody (_id,b); }
|
||||
|
||||
dBodyID getBody()
|
||||
{ return dGeomGetBody (_id); }
|
||||
|
||||
void setPosition (dReal x, dReal y, dReal z)
|
||||
{ dGeomSetPosition (_id,x,y,z); }
|
||||
|
||||
void setRotation (const dMatrix3 R)
|
||||
{ dGeomSetRotation (_id,R); }
|
||||
|
||||
const dReal * getPosition()
|
||||
{ return dGeomGetPosition (_id); }
|
||||
|
||||
const dReal * getRotation()
|
||||
{ return dGeomGetRotation (_id); }
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
217
extern/ode/dist/include/ode/odemath.h
vendored
217
extern/ode/dist/include/ode/odemath.h
vendored
@@ -1,217 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef _ODE_ODEMATH_H_
|
||||
#define _ODE_ODEMATH_H_
|
||||
|
||||
#include <ode/common.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
/* 3-way dot product. dDOTpq means that elements of `a' and `b' are spaced
|
||||
* p and q indexes apart respectively. dDOT() means dDOT11.
|
||||
*/
|
||||
|
||||
#ifdef __cplusplus
|
||||
inline dReal dDOT (const dReal *a, const dReal *b)
|
||||
{ return ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2]); }
|
||||
inline dReal dDOT14(const dReal *a, const dReal *b)
|
||||
{ return ((a)[0]*(b)[0] + (a)[1]*(b)[4] + (a)[2]*(b)[8]); }
|
||||
inline dReal dDOT41(const dReal *a, const dReal *b)
|
||||
{ return ((a)[0]*(b)[0] + (a)[4]*(b)[1] + (a)[8]*(b)[2]); }
|
||||
inline dReal dDOT44(const dReal *a, const dReal *b)
|
||||
{ return ((a)[0]*(b)[0] + (a)[4]*(b)[4] + (a)[8]*(b)[8]); }
|
||||
#else
|
||||
#define dDOT(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[1] + (a)[2]*(b)[2])
|
||||
#define dDOT14(a,b) ((a)[0]*(b)[0] + (a)[1]*(b)[4] + (a)[2]*(b)[8])
|
||||
#define dDOT41(a,b) ((a)[0]*(b)[0] + (a)[4]*(b)[1] + (a)[8]*(b)[2])
|
||||
#define dDOT44(a,b) ((a)[0]*(b)[0] + (a)[4]*(b)[4] + (a)[8]*(b)[8])
|
||||
#endif
|
||||
|
||||
|
||||
/* cross product, set a = b x c. dCROSSpqr means that elements of `a', `b'
|
||||
* and `c' are spaced p, q and r indexes apart respectively.
|
||||
* dCROSS() means dCROSS111. `op' is normally `=', but you can set it to
|
||||
* +=, -= etc to get other effects.
|
||||
*/
|
||||
|
||||
#define dCROSS(a,op,b,c) \
|
||||
(a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \
|
||||
(a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \
|
||||
(a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]);
|
||||
#define dCROSSpqr(a,op,b,c,p,q,r) \
|
||||
(a)[ 0] op ((b)[ q]*(c)[2*r] - (b)[2*q]*(c)[ r]); \
|
||||
(a)[ p] op ((b)[2*q]*(c)[ 0] - (b)[ 0]*(c)[2*r]); \
|
||||
(a)[2*p] op ((b)[ 0]*(c)[ r] - (b)[ q]*(c)[ 0]);
|
||||
#define dCROSS114(a,op,b,c) dCROSSpqr(a,op,b,c,1,1,4)
|
||||
#define dCROSS141(a,op,b,c) dCROSSpqr(a,op,b,c,1,4,1)
|
||||
#define dCROSS144(a,op,b,c) dCROSSpqr(a,op,b,c,1,4,4)
|
||||
#define dCROSS411(a,op,b,c) dCROSSpqr(a,op,b,c,4,1,1)
|
||||
#define dCROSS414(a,op,b,c) dCROSSpqr(a,op,b,c,4,1,4)
|
||||
#define dCROSS441(a,op,b,c) dCROSSpqr(a,op,b,c,4,4,1)
|
||||
#define dCROSS444(a,op,b,c) dCROSSpqr(a,op,b,c,4,4,4)
|
||||
|
||||
|
||||
/* set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b.
|
||||
* A is stored by rows, and has `skip' elements per row. the matrix is
|
||||
* assumed to be already zero, so this does not write zero elements!
|
||||
* if (plus,minus) is (+,-) then a positive version will be written.
|
||||
* if (plus,minus) is (-,+) then a negative version will be written.
|
||||
*/
|
||||
|
||||
#define dCROSSMAT(A,a,skip,plus,minus) \
|
||||
(A)[1] = minus (a)[2]; \
|
||||
(A)[2] = plus (a)[1]; \
|
||||
(A)[(skip)+0] = plus (a)[2]; \
|
||||
(A)[(skip)+2] = minus (a)[0]; \
|
||||
(A)[2*(skip)+0] = minus (a)[1]; \
|
||||
(A)[2*(skip)+1] = plus (a)[0];
|
||||
|
||||
|
||||
/* compute the distance between two 3-vectors (oops, C++!) */
|
||||
#ifdef __cplusplus
|
||||
inline dReal dDISTANCE (const dVector3 a, const dVector3 b)
|
||||
{ return dSqrt( (a[0]-b[0])*(a[0]-b[0]) + (a[1]-b[1])*(a[1]-b[1]) +
|
||||
(a[2]-b[2])*(a[2]-b[2]) ); }
|
||||
#else
|
||||
#define dDISTANCE(a,b) \
|
||||
(dSqrt( ((a)[0]-(b)[0])*((a)[0]-(b)[0]) + ((a)[1]-(b)[1])*((a)[1]-(b)[1]) + \
|
||||
((a)[2]-(b)[2])*((a)[2]-(b)[2]) ))
|
||||
#endif
|
||||
|
||||
|
||||
/* normalize 3x1 and 4x1 vectors (i.e. scale them to unit length) */
|
||||
void dNormalize3 (dVector3 a);
|
||||
void dNormalize4 (dVector4 a);
|
||||
|
||||
|
||||
/* given a unit length "normal" vector n, generate vectors p and q vectors
|
||||
* that are an orthonormal basis for the plane space perpendicular to n.
|
||||
* i.e. this makes p,q such that n,p,q are all perpendicular to each other.
|
||||
* q will equal n x p. if n is not unit length then p will be unit length but
|
||||
* q wont be.
|
||||
*/
|
||||
|
||||
void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q);
|
||||
|
||||
|
||||
/* special case matrix multipication, with operator selection */
|
||||
|
||||
#define dMULTIPLYOP0_331(A,op,B,C) \
|
||||
(A)[0] op dDOT((B),(C)); \
|
||||
(A)[1] op dDOT((B+4),(C)); \
|
||||
(A)[2] op dDOT((B+8),(C));
|
||||
#define dMULTIPLYOP1_331(A,op,B,C) \
|
||||
(A)[0] op dDOT41((B),(C)); \
|
||||
(A)[1] op dDOT41((B+1),(C)); \
|
||||
(A)[2] op dDOT41((B+2),(C));
|
||||
#define dMULTIPLYOP0_133(A,op,B,C) \
|
||||
(A)[0] op dDOT14((B),(C)); \
|
||||
(A)[1] op dDOT14((B),(C+1)); \
|
||||
(A)[2] op dDOT14((B),(C+2));
|
||||
#define dMULTIPLYOP0_333(A,op,B,C) \
|
||||
(A)[0] op dDOT14((B),(C)); \
|
||||
(A)[1] op dDOT14((B),(C+1)); \
|
||||
(A)[2] op dDOT14((B),(C+2)); \
|
||||
(A)[4] op dDOT14((B+4),(C)); \
|
||||
(A)[5] op dDOT14((B+4),(C+1)); \
|
||||
(A)[6] op dDOT14((B+4),(C+2)); \
|
||||
(A)[8] op dDOT14((B+8),(C)); \
|
||||
(A)[9] op dDOT14((B+8),(C+1)); \
|
||||
(A)[10] op dDOT14((B+8),(C+2));
|
||||
#define dMULTIPLYOP1_333(A,op,B,C) \
|
||||
(A)[0] op dDOT44((B),(C)); \
|
||||
(A)[1] op dDOT44((B),(C+1)); \
|
||||
(A)[2] op dDOT44((B),(C+2)); \
|
||||
(A)[4] op dDOT44((B+1),(C)); \
|
||||
(A)[5] op dDOT44((B+1),(C+1)); \
|
||||
(A)[6] op dDOT44((B+1),(C+2)); \
|
||||
(A)[8] op dDOT44((B+2),(C)); \
|
||||
(A)[9] op dDOT44((B+2),(C+1)); \
|
||||
(A)[10] op dDOT44((B+2),(C+2));
|
||||
#define dMULTIPLYOP2_333(A,op,B,C) \
|
||||
(A)[0] op dDOT((B),(C)); \
|
||||
(A)[1] op dDOT((B),(C+4)); \
|
||||
(A)[2] op dDOT((B),(C+8)); \
|
||||
(A)[4] op dDOT((B+4),(C)); \
|
||||
(A)[5] op dDOT((B+4),(C+4)); \
|
||||
(A)[6] op dDOT((B+4),(C+8)); \
|
||||
(A)[8] op dDOT((B+8),(C)); \
|
||||
(A)[9] op dDOT((B+8),(C+4)); \
|
||||
(A)[10] op dDOT((B+8),(C+8));
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
||||
inline void dMULTIPLY0_331(dReal *A, const dReal *B, const dReal *C)
|
||||
{ dMULTIPLYOP0_331(A,=,B,C) }
|
||||
inline void dMULTIPLY1_331(dReal *A, const dReal *B, const dReal *C)
|
||||
{ dMULTIPLYOP1_331(A,=,B,C) }
|
||||
inline void dMULTIPLY0_133(dReal *A, const dReal *B, const dReal *C)
|
||||
{ dMULTIPLYOP0_133(A,=,B,C) }
|
||||
inline void dMULTIPLY0_333(dReal *A, const dReal *B, const dReal *C)
|
||||
{ dMULTIPLYOP0_333(A,=,B,C) }
|
||||
inline void dMULTIPLY1_333(dReal *A, const dReal *B, const dReal *C)
|
||||
{ dMULTIPLYOP1_333(A,=,B,C) }
|
||||
inline void dMULTIPLY2_333(dReal *A, const dReal *B, const dReal *C)
|
||||
{ dMULTIPLYOP2_333(A,=,B,C) }
|
||||
|
||||
inline void dMULTIPLYADD0_331(dReal *A, const dReal *B, const dReal *C)
|
||||
{ dMULTIPLYOP0_331(A,+=,B,C) }
|
||||
inline void dMULTIPLYADD1_331(dReal *A, const dReal *B, const dReal *C)
|
||||
{ dMULTIPLYOP1_331(A,+=,B,C) }
|
||||
inline void dMULTIPLYADD0_133(dReal *A, const dReal *B, const dReal *C)
|
||||
{ dMULTIPLYOP0_133(A,+=,B,C) }
|
||||
inline void dMULTIPLYADD0_333(dReal *A, const dReal *B, const dReal *C)
|
||||
{ dMULTIPLYOP0_333(A,+=,B,C) }
|
||||
inline void dMULTIPLYADD1_333(dReal *A, const dReal *B, const dReal *C)
|
||||
{ dMULTIPLYOP1_333(A,+=,B,C) }
|
||||
inline void dMULTIPLYADD2_333(dReal *A, const dReal *B, const dReal *C)
|
||||
{ dMULTIPLYOP2_333(A,+=,B,C) }
|
||||
|
||||
#else
|
||||
|
||||
#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C)
|
||||
#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C)
|
||||
#define dMULTIPLY0_133(A,B,C) dMULTIPLYOP0_133(A,=,B,C)
|
||||
#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C)
|
||||
#define dMULTIPLY1_333(A,B,C) dMULTIPLYOP1_333(A,=,B,C)
|
||||
#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C)
|
||||
|
||||
#define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C)
|
||||
#define dMULTIPLYADD1_331(A,B,C) dMULTIPLYOP1_331(A,+=,B,C)
|
||||
#define dMULTIPLYADD0_133(A,B,C) dMULTIPLYOP0_133(A,+=,B,C)
|
||||
#define dMULTIPLYADD0_333(A,B,C) dMULTIPLYOP0_333(A,+=,B,C)
|
||||
#define dMULTIPLYADD1_333(A,B,C) dMULTIPLYOP1_333(A,+=,B,C)
|
||||
#define dMULTIPLYADD2_333(A,B,C) dMULTIPLYOP2_333(A,+=,B,C)
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
65
extern/ode/dist/include/ode/rotation.h
vendored
65
extern/ode/dist/include/ode/rotation.h
vendored
@@ -1,65 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef _ODE_ROTATION_H_
|
||||
#define _ODE_ROTATION_H_
|
||||
|
||||
#include <ode/common.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
void dRSetIdentity (dMatrix3 R);
|
||||
|
||||
void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az,
|
||||
dReal angle);
|
||||
|
||||
void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi);
|
||||
|
||||
void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az,
|
||||
dReal bx, dReal by, dReal bz);
|
||||
|
||||
void dQSetIdentity (dQuaternion q);
|
||||
|
||||
void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
|
||||
dReal angle);
|
||||
|
||||
void dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc);
|
||||
void dQMultiply1 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc);
|
||||
void dQMultiply2 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc);
|
||||
void dQMultiply3 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc);
|
||||
|
||||
void dQtoR (const dQuaternion q, dMatrix3 R);
|
||||
|
||||
void dRtoQ (const dMatrix3 R, dQuaternion q);
|
||||
|
||||
void dWtoDQ (const dVector3 w, const dQuaternion q, dVector4 dq);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
78
extern/ode/dist/include/ode/space.h
vendored
78
extern/ode/dist/include/ode/space.h
vendored
@@ -1,78 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef _ODE_SPACE_H_
|
||||
#define _ODE_SPACE_H_
|
||||
|
||||
#include <ode/common.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
struct dContactGeom;
|
||||
|
||||
typedef void dNearCallback (void *data, dGeomID o1, dGeomID o2);
|
||||
|
||||
|
||||
/* extra information the space needs in every geometry object */
|
||||
|
||||
typedef struct dGeomSpaceData {
|
||||
dGeomID next;
|
||||
} dGeomSpaceData;
|
||||
|
||||
|
||||
dSpaceID dSimpleSpaceCreate();
|
||||
dSpaceID dHashSpaceCreate();
|
||||
|
||||
void dSpaceDestroy (dSpaceID);
|
||||
void dSpaceAdd (dSpaceID, dGeomID);
|
||||
void dSpaceRemove (dSpaceID, dGeomID);
|
||||
void dSpaceCollide (dSpaceID space, void *data, dNearCallback *callback);
|
||||
int dSpaceQuery (dSpaceID, dGeomID);
|
||||
|
||||
void dHashSpaceSetLevels (dSpaceID space, int minlevel, int maxlevel);
|
||||
|
||||
|
||||
/* @@@ NOT FLEXIBLE ENOUGH
|
||||
*
|
||||
* generate contacts for those objects in the space that touch each other.
|
||||
* an array of contacts is created on the alternative stack using
|
||||
* StackAlloc(), and a pointer to the array is returned. the size of the
|
||||
* array is returned by the function.
|
||||
*/
|
||||
/* int dSpaceCollide (dSpaceID space, dContactGeom **contact_array); */
|
||||
|
||||
|
||||
/* HMMMMM... i dont think so.
|
||||
* tell the space that an object has moved, so its representation in the
|
||||
* space should be changed.
|
||||
*/
|
||||
/* void dSpaceObjectMoved (dSpaceID, dGeomID); */
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
77
extern/ode/dist/include/ode/timer.h
vendored
77
extern/ode/dist/include/ode/timer.h
vendored
@@ -1,77 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef _ODE_TIMER_H_
|
||||
#define _ODE_TIMER_H_
|
||||
|
||||
#include <ode/config.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
/* stop watch objects */
|
||||
|
||||
typedef struct dStopwatch {
|
||||
double time; /* total clock count */
|
||||
unsigned long cc[2]; /* clock count since last `start' */
|
||||
} dStopwatch;
|
||||
|
||||
void dStopwatchReset (dStopwatch *);
|
||||
void dStopwatchStart (dStopwatch *);
|
||||
void dStopwatchStop (dStopwatch *);
|
||||
double dStopwatchTime (dStopwatch *); /* returns total time in secs */
|
||||
|
||||
|
||||
/* code timers */
|
||||
|
||||
void dTimerStart (const char *description); /* pass a static string here */
|
||||
void dTimerNow (const char *description); /* pass a static string here */
|
||||
void dTimerEnd();
|
||||
|
||||
/* print out a timer report. if `average' is nonzero, print out the average
|
||||
* time for each slot (this is only meaningful if the same start-now-end
|
||||
* calls are being made repeatedly.
|
||||
*/
|
||||
void dTimerReport (FILE *fout, int average);
|
||||
|
||||
|
||||
/* resolution */
|
||||
|
||||
/* returns the timer ticks per second implied by the timing hardware or API.
|
||||
* the actual timer resolution may not be this great.
|
||||
*/
|
||||
double dTimerTicksPerSecond();
|
||||
|
||||
/* returns an estimate of the actual timer resolution, in seconds. this may
|
||||
* be greater than 1/ticks_per_second.
|
||||
*/
|
||||
double dTimerResolution();
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
158
extern/ode/dist/ode/README
vendored
158
extern/ode/dist/ode/README
vendored
@@ -1,158 +0,0 @@
|
||||
Dynamics Library.
|
||||
=================
|
||||
|
||||
CONVENTIONS
|
||||
-----------
|
||||
|
||||
matrix storage
|
||||
--------------
|
||||
|
||||
matrix operations like factorization are expensive, so we must store the data
|
||||
in a way that is most useful to the matrix code. we want the ability to update
|
||||
the dynamics library without recompiling applications, e.g. so users can take
|
||||
advantage of new floating point hardware. so we must settle on a single
|
||||
format. because of the prevalence of 4-way SIMD, the format is this: store
|
||||
the matrix by rows or columns, and each column is rounded up to a multiple of
|
||||
4 elements. the extra "padding" elements at the end of each row/column are set
|
||||
to 0. this is called the "standard format". to indicate if the data is stored
|
||||
by rows or columns, we will say "standard row format" or "standard column
|
||||
format". hopefully this decision will remain good in the future, as more and
|
||||
more processors have 4-way SIMD, and 3D graphics always needs fast 4x4
|
||||
matrices.
|
||||
|
||||
exception: matrices that have only one column or row (vectors), are always
|
||||
stored as consecutive elements in standard row format, i.e. there is no
|
||||
interior padding, only padding at the end.
|
||||
|
||||
thus: all 3x1 floating point vectors are stored as 4x1 vectors: (x,x,x,0).
|
||||
also: all 6x1 spatial velocities and accelerations are split into 3x1 position
|
||||
and angular components, which are stored as contiguous 4x1 vectors.
|
||||
|
||||
ALL matrices are stored by in standard row format.
|
||||
|
||||
|
||||
arguments
|
||||
---------
|
||||
|
||||
3x1 vector arguments to set() functions are supplied as x,y,z.
|
||||
3x1 vector result arguments to get() function are pointers to arrays.
|
||||
larger vectors are always supplied and returned as pointers.
|
||||
all coordinates are in the global frame except where otherwise specified.
|
||||
output-only arguments are usually supplied at the end.
|
||||
|
||||
|
||||
memory allocation
|
||||
-----------------
|
||||
|
||||
with many C/C++ libraries memory allocation is a difficult problem to solve.
|
||||
who allocates the memory? who frees it? must objects go on the heap or can
|
||||
they go on the stack or in static storage? to provide the maximum flexibility,
|
||||
the dynamics and collision libraries do not do their own memory allocation.
|
||||
you must pass in pointers to externally allocated chunks of the right sizes.
|
||||
the body, joint and colllision object structures are all exported, so you
|
||||
can make instances of those structure and pass pointers to them.
|
||||
|
||||
there are helper functions which allocate objects out of areans, in case you
|
||||
need loots of dynamic creation and deletion.
|
||||
|
||||
BUT!!! this ties us down to the body/joint/collision representation.
|
||||
|
||||
a better approach is to supply custom memory allocation functions
|
||||
(e.g. dlAlloc() etc).
|
||||
|
||||
|
||||
C versus C++ ... ?
|
||||
------------------
|
||||
|
||||
everything should be C linkable, and there should be C header files for
|
||||
everything. but we want to develop in C++. so do this:
|
||||
* all comments are "//". automatically convert to /**/ for distribution.
|
||||
* structures derived from other structures --> automatically convert?
|
||||
|
||||
|
||||
WORLDS
|
||||
------
|
||||
|
||||
might want better terminology here.
|
||||
|
||||
the dynamics world (DWorld) is a list of systems. each system corresponds to
|
||||
one or more bodies, or perhaps some other kinds of physical object.
|
||||
each system corresponds to one or more objects in the collision world
|
||||
(there does not have to be a one-to-one correspondence between bodies and
|
||||
collision objects).
|
||||
|
||||
systems are simulated separately, perhaps using completely different
|
||||
techniques. we must do something special when systems collide.
|
||||
systems collide when collision objects belonging to system A touch
|
||||
collision objects belonging to system B.
|
||||
|
||||
for each collision point, the system must provide matrix equation data
|
||||
that is used to compute collision forces. once those forces are computed,
|
||||
the system must incorporate the forces into its timestep.
|
||||
PROBLEM: what if we intertwine the LCP problems of the two systems - then
|
||||
this simple approach wont work.
|
||||
|
||||
the dynamics world contains two kinds of objects: bodies and joints.
|
||||
joints connect two bodies together.
|
||||
|
||||
the world contains one of more partitions. each partition is a collection of
|
||||
bodies and joints such that each body is attached (through one or more joints)
|
||||
to every other body.
|
||||
|
||||
Joints
|
||||
------
|
||||
|
||||
a joint can be connected to one or two bodies.
|
||||
if the joint is only connected to one body, joint.node[1].body == 0.
|
||||
joint.node[0].body is always valid.
|
||||
|
||||
|
||||
Linkage
|
||||
-------
|
||||
|
||||
this library will always be statically linked with the app, for these reasons:
|
||||
* collision space is selected at compile time, it adds data to the geom
|
||||
objects.
|
||||
|
||||
|
||||
Optimization
|
||||
------------
|
||||
|
||||
doubles must be aligned on 8 byte boundaries!
|
||||
|
||||
|
||||
MinGW on Windows issues
|
||||
-----------------------
|
||||
|
||||
* the .rc file for drawstuff needs a different include, try winresrc.h.
|
||||
|
||||
* it seems we can't have both main() and WinMain() without the entry point
|
||||
defaulting to main() and having resource loading problems. this screws up
|
||||
what i was trying to do in the drawstuff library. perhaps main2() ?
|
||||
|
||||
* remember to compile resources to COFF format RES files.
|
||||
|
||||
|
||||
|
||||
Collision
|
||||
---------
|
||||
|
||||
to plug in your own collision handling, replace (some of?) these functions
|
||||
with your own. collision should be a separate library that you can link in
|
||||
or not. your own library can call components in this collision library, e.g.
|
||||
if you want polymorphic spaces instead of a single statically called space.
|
||||
|
||||
creating an object will automatically register the appropriate
|
||||
class (if necessary). how can we ensure that the minimum amount of code is
|
||||
linked in? e.g. only one space handler, and sphere-sphere and sphere-box and
|
||||
box-box collision code (if spheres and boxes instanced).
|
||||
|
||||
the user creates a collision space, and for each dynamics object that is
|
||||
created a collision object is inserted into the space. the collision
|
||||
object's pos and R pointers are set to the corresponding dynamics
|
||||
variables.
|
||||
|
||||
there should be utility functions which create the dynamics and collision
|
||||
objects at the same time, e.g. dMakeSphere().
|
||||
|
||||
collision objects and dynamics objects keep pointers to each other.
|
||||
148
extern/ode/dist/ode/fbuild/BuildDot
vendored
148
extern/ode/dist/ode/fbuild/BuildDot
vendored
@@ -1,148 +0,0 @@
|
||||
#!/usr/bin/perl
|
||||
#
|
||||
#########################################################################
|
||||
# #
|
||||
# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. #
|
||||
# All rights reserved. Email: russ@q12.org Web: www.q12.org #
|
||||
# #
|
||||
# This library is free software; you can redistribute it and/or #
|
||||
# modify it under the terms of EITHER: #
|
||||
# (1) The GNU Lesser General Public License as published by the Free #
|
||||
# Software Foundation; either version 2.1 of the License, or (at #
|
||||
# your option) any later version. The text of the GNU Lesser #
|
||||
# General Public License is included with this library in the #
|
||||
# file LICENSE.TXT. #
|
||||
# (2) The BSD-style license that is included with this library in #
|
||||
# the file LICENSE-BSD.TXT. #
|
||||
# #
|
||||
# This library is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files #
|
||||
# LICENSE.TXT and LICENSE-BSD.TXT for more details. #
|
||||
# #
|
||||
#########################################################################
|
||||
|
||||
# dot product code generator.
|
||||
#
|
||||
# code generation parameters, set in a parameters file:
|
||||
# FNAME : name of source file to generate - a .c file will be made
|
||||
# UNROLL1 : inner loop unrolling factor (1..)
|
||||
# FETCH : max num of a[i]'s and b[i]'s to load ahead of muls
|
||||
# LAT1 : load -> mul latency (>=1)
|
||||
# LAT2 : mul -> add latency (>=1). if this is 1, use fused mul-add
|
||||
#
|
||||
#############################################################################
|
||||
|
||||
require ("BuildUtil");
|
||||
|
||||
# get and check code generation parameters
|
||||
error ("Usage: BuildDot <parameters-file>") if $#ARGV != 0;
|
||||
do $ARGV[0];
|
||||
|
||||
if (!defined($FNAME) || !defined($UNROLL1) || !defined($FETCH) ||
|
||||
!defined($LAT1) || !defined($LAT2)) {
|
||||
error ("code generation parameters not defined");
|
||||
}
|
||||
|
||||
# check parameters
|
||||
error ("bad UNROLL1") if $UNROLL1 < 1;
|
||||
error ("bad FETCH") if $FETCH < 1;
|
||||
error ("bad LAT1") if $LAT1 < 1;
|
||||
error ("bad LAT2") if $LAT2 < 1;
|
||||
|
||||
#############################################################################
|
||||
|
||||
open (FOUT,">$FNAME.c") or die "can't open $FNAME.c for writing";
|
||||
|
||||
# file and function header
|
||||
output (<<END);
|
||||
/* generated code, do not edit. */
|
||||
|
||||
#include "ode/matrix.h"
|
||||
|
||||
|
||||
dReal dDot (const dReal *a, const dReal *b, int n)
|
||||
{
|
||||
END
|
||||
|
||||
output ("dReal ");
|
||||
for ($i=0; $i<$UNROLL1; $i++) {
|
||||
output ("p$i,q$i,m$i,");
|
||||
}
|
||||
output ("sum;\n");
|
||||
|
||||
output (<<END);
|
||||
sum = 0;
|
||||
n -= $UNROLL1;
|
||||
while (n >= 0) {
|
||||
END
|
||||
|
||||
@load = (); # slot where a[i]'s and b[i]'s loaded
|
||||
@mul = (); # slot where multiply i happened
|
||||
@add = (); # slow where add i happened
|
||||
|
||||
# in the future we may want to reduce the number of variables declared,
|
||||
# so these arrays will be useful.
|
||||
@pqused = (); # 1 if p/q[i] loaded with data, 0 once that data's used
|
||||
@mused = (); # 1 if m[i] loaded with data, 0 once that data's used
|
||||
@pqmap = (); # map virtual p/q variables to actual p/q variables
|
||||
@mmap = (); # map virtual m variables to actual m variables
|
||||
|
||||
output ("p0 = a[0]; q0 = b[0];\n");
|
||||
push (@load,0);
|
||||
|
||||
$slot=0; # one slot for every load/mul/add/nop issued
|
||||
for (;;) {
|
||||
$startslot = $slot;
|
||||
|
||||
# do next load
|
||||
if (($#load - $#mul) < $FETCH && ($#load+1) < $UNROLL1) {
|
||||
push (@load,$slot);
|
||||
output ("p$#load = a[$#load]; q$#load = b[$#load];\n");
|
||||
$slot++;
|
||||
}
|
||||
# do next multiply
|
||||
if ($#load > $#mul && $slot >= ($load[$#mul+1] + $LAT1) &&
|
||||
($#mul+1) < $UNROLL1) {
|
||||
push (@mul,$slot);
|
||||
if ($LAT2 > 1) {
|
||||
output ("m$#mul = p$#mul * q$#mul;\n");
|
||||
}
|
||||
else {
|
||||
output ("sum += p$#mul * q$#mul;\n");
|
||||
last if ($#mul+1) >= $UNROLL1;
|
||||
}
|
||||
$slot++;
|
||||
}
|
||||
# do next add
|
||||
if ($LAT2 > 1) {
|
||||
if ($#mul > $#add && $slot >= ($mul[$#add+1] + $LAT2)) {
|
||||
push (@add,$slot);
|
||||
output ("sum += m$#add;\n");
|
||||
$slot++;
|
||||
last if ($#add+1) >= $UNROLL1;
|
||||
}
|
||||
}
|
||||
|
||||
if ($slot == $startslot) {
|
||||
# comment ("nop");
|
||||
$slot++;
|
||||
}
|
||||
}
|
||||
|
||||
output ("a += $UNROLL1;\n");
|
||||
output ("b += $UNROLL1;\n");
|
||||
output ("n -= $UNROLL1;\n");
|
||||
output ("}\n");
|
||||
|
||||
output (<<END);
|
||||
n += $UNROLL1;
|
||||
while (n > 0) {
|
||||
sum += (*a) * (*b);
|
||||
a++;
|
||||
b++;
|
||||
n--;
|
||||
}
|
||||
return sum;
|
||||
}
|
||||
END
|
||||
654
extern/ode/dist/ode/fbuild/BuildLDLT
vendored
654
extern/ode/dist/ode/fbuild/BuildLDLT
vendored
@@ -1,654 +0,0 @@
|
||||
#!/usr/bin/perl
|
||||
#
|
||||
#########################################################################
|
||||
# #
|
||||
# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. #
|
||||
# All rights reserved. Email: russ@q12.org Web: www.q12.org #
|
||||
# #
|
||||
# This library is free software; you can redistribute it and/or #
|
||||
# modify it under the terms of EITHER: #
|
||||
# (1) The GNU Lesser General Public License as published by the Free #
|
||||
# Software Foundation; either version 2.1 of the License, or (at #
|
||||
# your option) any later version. The text of the GNU Lesser #
|
||||
# General Public License is included with this library in the #
|
||||
# file LICENSE.TXT. #
|
||||
# (2) The BSD-style license that is included with this library in #
|
||||
# the file LICENSE-BSD.TXT. #
|
||||
# #
|
||||
# This library is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files #
|
||||
# LICENSE.TXT and LICENSE-BSD.TXT for more details. #
|
||||
# #
|
||||
#########################################################################
|
||||
|
||||
#
|
||||
# triangular matrix solver and factorizer code generator.
|
||||
#
|
||||
# SOLVER
|
||||
# ------
|
||||
#
|
||||
# if L is an n x n lower triangular matrix (with ones on the diagonal), the
|
||||
# solver solves L*X=B where X and B are n x m matrices. this is the core
|
||||
# step in L*D*L' factorization. the algorithm is (in matlab):
|
||||
#
|
||||
# for i=1:n
|
||||
# for j=1:m
|
||||
# X(i,j) = B(i,j) - L(i,1:i-1)*X(1:i-1,j);
|
||||
# end
|
||||
# end
|
||||
#
|
||||
# note that the ordering of the (i,j) loop is somewhat arbitrary. the only
|
||||
# prerequisite to calculating element (i,j) of X is that all X(1:i-1,j) have
|
||||
# have already been calcuated. this gives us some flexibility.
|
||||
#
|
||||
# the code generated below calculates X in N1 x N1 blocks. to speed up the
|
||||
# innermost dot product loop, the outer product trick is used. for instance,
|
||||
# to calculate the value of the 2x2 matrix ABCD below we first iterate over
|
||||
# the vectors (a,b,c,d) and (e,f,g,h), computing ABCD = a*e+b*f+c*g+d*h.
|
||||
# then A and B contain the dot product values needed in the algorithm, and
|
||||
# C and D have almost all of it. the outer product trick reduces the number
|
||||
# of memory loads required. in this example 16 loads are required, but if
|
||||
# the simple dot product in the above algorithm is used then 32 loads are
|
||||
# required. increasing N1 decreases the total number of loads, but only as long
|
||||
# as we have enough temporary registers to keep the matrix blocks and vectors.
|
||||
#
|
||||
# L * X = B
|
||||
#
|
||||
# [ . ] [ e e ] [ . . ]
|
||||
# [ . . ] [ f f ] [ . . ]
|
||||
# [ . . . ] [ g g ] [ . . ]
|
||||
# [ . . . . ] [ h h ] [ . . ]
|
||||
# [ a b c d . ] [ A B ] = [ . . ]
|
||||
# [ a b c d . . ] [ C D ] [ . . ]
|
||||
# [ . . . . . . . ] [ . . ] [ . . ]
|
||||
# [ . . . . . . . . ] [ . . ] [ . . ]
|
||||
# [ . . . . . . . . . ] [ . . ] [ . . ]
|
||||
#
|
||||
# note that L is stored by rows but X and B are stored by columns.
|
||||
# the outer product loops are unrolled for extra speed.
|
||||
#
|
||||
# LDLT FACTORIZATION
|
||||
# ------------------
|
||||
#
|
||||
# the factorization algorithm builds L incrementally by repeatedly solving
|
||||
# the following equation:
|
||||
#
|
||||
# [ L 0 ] [ D 0 ] [ L' l ] = [ A a ] <-- n rows
|
||||
# [ l' e ] [ 0 d ] [ 0 e' ] [ a' b ] <-- m rows
|
||||
#
|
||||
# [ L*D*L' L*D*l ] = [ A a ]
|
||||
# [ l'*D*L' l'*D*l+e*d*e' ] [ a' b ]
|
||||
#
|
||||
# L*D*L'=A is an existing solution, and a,b are new rows/columns to add to A.
|
||||
# we compute:
|
||||
#
|
||||
# L * (Dl) = a
|
||||
# l = inv(D) * Dl
|
||||
# e*d*e' = b - l'*Dl (m*m LDLT factorization)
|
||||
#
|
||||
#
|
||||
# L-transpose solver
|
||||
# ------------------
|
||||
#
|
||||
# the LT (L-transpose) solver uses the same logic as the standard L-solver,
|
||||
# with a few tricks to make it work. to solve L^T*X=B we first remap:
|
||||
# L to Lhat : Lhat(i,j) = L(n-j,n-i)
|
||||
# X to Xhat : Xhat(i) = X(n-i)
|
||||
# B to Bhat : Bhat(i) = B(n-i)
|
||||
# and then solve Lhat*Xhat = Bhat. the current LT solver only supports one
|
||||
# right hand side, but that's okay as it is not used in the factorizer.
|
||||
#
|
||||
#############################################################################
|
||||
#
|
||||
# code generation parameters, set in a parameters file:
|
||||
# FNAME : name of source file to generate - a .c file will be made
|
||||
# TYPE : 'f' to build factorizer, 's' to build solver, 't' to build the
|
||||
# transpose solver.
|
||||
# N1 : block size (size of outer product matrix) (1..9)
|
||||
# UNROLL1 : solver inner loop unrolling factor (1..)
|
||||
# UNROLL2 : factorizer inner loop unrolling factor (1..)
|
||||
# MADD : if nonzero, generate code for fused multiply-add (0,1)
|
||||
# FETCH : how to fetch data in the inner loop:
|
||||
# 0 - load in a batch (the `normal way')
|
||||
# 1 - delay inner loop loads until just before they're needed
|
||||
#
|
||||
#############################################################################
|
||||
#
|
||||
# TODO
|
||||
# ----
|
||||
#
|
||||
# * dFactorLDLT() is not so efficient for matrix sizes < block size, e.g.
|
||||
# redundant calls, zero loads, adds etc
|
||||
#
|
||||
#############################################################################
|
||||
#
|
||||
# NOTES:
|
||||
#
|
||||
# * on the pentium we can prefetch like this:
|
||||
# asm ("prefetcht0 %0" : : "m" (*Ai) );
|
||||
# but it doesn't seem to help much
|
||||
|
||||
require ("BuildUtil");
|
||||
|
||||
# get and check code generation parameters
|
||||
error ("Usage: BuildLDLT <parameters-file>") if $#ARGV != 0;
|
||||
do $ARGV[0];
|
||||
|
||||
if (!defined($FNAME) || !defined($TYPE) || !defined($N1) ||
|
||||
!defined($UNROLL1) || !defined($UNROLL2) || !defined($MADD) ||
|
||||
!defined($FETCH)) {
|
||||
error ("code generation parameters not defined");
|
||||
}
|
||||
|
||||
# check parameters
|
||||
error ("bad TYPE") if $TYPE ne 'f' && $TYPE ne 's' && $TYPE ne 't';
|
||||
error ("bad N1") if $N1 < 1 || $N1 > 9;
|
||||
error ("bad UNROLL1") if $UNROLL1 < 1;
|
||||
error ("bad UNROLL2") if $UNROLL2 < 1;
|
||||
error ("bad MADD") if $MADD != 0 && $MADD != 1;
|
||||
error ("bad FETCH") if $FETCH < 0 && $FETCH > 1;
|
||||
|
||||
#############################################################################
|
||||
# utility
|
||||
|
||||
# functions to handle delayed loading of p and q values.
|
||||
# bit in the the `ploaded' and `qloaded' numbers record what has been loaded,
|
||||
# so we dont load it again.
|
||||
|
||||
sub newLoads
|
||||
{
|
||||
# bits in these numbers say what registers p and q have been loaded so far
|
||||
$ploaded = 0;
|
||||
$qloaded = 0;
|
||||
}
|
||||
|
||||
sub loadedEverything
|
||||
{
|
||||
$ploaded = 0xffffffff;
|
||||
$qloaded = 0xffffffff;
|
||||
}
|
||||
|
||||
sub loadP # (i,loadcmd)
|
||||
{
|
||||
my $i = $_[0];
|
||||
my $loadcmd = $_[1];
|
||||
return if ($ploaded & (1 << $i));
|
||||
output ($loadcmd);
|
||||
$ploaded |= (1 << $i);
|
||||
}
|
||||
|
||||
sub loadQ # (i,loadcmd)
|
||||
{
|
||||
my $i = $_[0];
|
||||
my $loadcmd = $_[1];
|
||||
return if ($qloaded & (1 << $i));
|
||||
output ($loadcmd);
|
||||
$qloaded |= (1 << $i);
|
||||
}
|
||||
|
||||
#############################################################################
|
||||
# make a fast L solve function.
|
||||
# this function has a restriction that the leading dimension of X and B must
|
||||
# be a multiple of the block size.
|
||||
|
||||
sub innerOuterProductLoop # (M,k,nrhs,increment)
|
||||
{
|
||||
my $M=$_[0];
|
||||
my $k=$_[1];
|
||||
my $nrhs=$_[2];
|
||||
my $increment=$_[3];
|
||||
my ($i,$j);
|
||||
newLoads;
|
||||
if ($FETCH==0) {
|
||||
comment ("load p and q values");
|
||||
for ($i=1; $i<=$M; $i++) {
|
||||
if ($TYPE eq 't') {
|
||||
output ("p$i=ell[".ofs2(-($i-1),0,'lskip')."];\n");
|
||||
output ("q$i=ex[".ofs2(-($k),$i-1,'lskip')."];\n") if $i <= $nrhs;
|
||||
}
|
||||
else {
|
||||
output ("p$i=ell[".ofs2($k,$i-1,'lskip')."];\n");
|
||||
output ("q$i=ex[".ofs2($k,$i-1,'lskip')."];\n") if $i <= $nrhs;
|
||||
}
|
||||
}
|
||||
loadedEverything;
|
||||
}
|
||||
|
||||
comment ("compute outer product and add it to the Z matrix");
|
||||
for ($i=1; $i<=$M; $i++) {
|
||||
for ($j=1; $j<=$nrhs; $j++) {
|
||||
if ($TYPE eq 't') {
|
||||
loadP ($i,"p$i=ell[".ofs2(-($i-1),0,'lskip')."];\n");
|
||||
loadQ ($j,"q$j=ex[".ofs2(-($k),$j-1,'lskip')."];\n");
|
||||
}
|
||||
else {
|
||||
loadP ($i,"p$i=ell[".ofs2($k,$i-1,'lskip')."];\n");
|
||||
loadQ ($j,"q$j=ex[".ofs2($k,$j-1,'lskip')."];\n");
|
||||
}
|
||||
my $var = $MADD ? "Z$i$j +=" : "m$i$j =";
|
||||
output ("$var p$i * q$j;\n");
|
||||
}
|
||||
}
|
||||
|
||||
if ($TYPE eq 't') {
|
||||
if ($increment > 0) {
|
||||
output ("ell += lskip1;\n");
|
||||
output ("ex -= $increment;\n");
|
||||
}
|
||||
else {
|
||||
output ("ell += lskip1;\n");
|
||||
}
|
||||
}
|
||||
else {
|
||||
if ($increment > 0) {
|
||||
comment ("advance pointers");
|
||||
output ("ell += $increment;\n");
|
||||
output ("ex += $increment;\n");
|
||||
}
|
||||
}
|
||||
|
||||
if ($MADD==0) {
|
||||
for ($i=1; $i<=$M; $i++) {
|
||||
for ($j=1; $j<=$nrhs; $j++) {
|
||||
output ("Z$i$j += m$i$j;\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
sub computeRows # (nrhs,rows)
|
||||
{
|
||||
my $nrhs = $_[0];
|
||||
my $rows = $_[1];
|
||||
my ($i,$j,$k);
|
||||
|
||||
comment ("compute all $rows x $nrhs block of X, from rows i..i+$rows-1");
|
||||
|
||||
comment ("set the Z matrix to 0");
|
||||
for ($i=1; $i<=$rows; $i++) {
|
||||
for ($j=1; $j<=$nrhs; $j++) {
|
||||
output ("Z$i$j=0;\n");
|
||||
}
|
||||
}
|
||||
if ($TYPE eq 't') {
|
||||
output ("ell = L - i;\n");
|
||||
}
|
||||
else {
|
||||
output ("ell = L + i*lskip1;\n");
|
||||
}
|
||||
output ("ex = B;\n");
|
||||
|
||||
comment ("the inner loop that computes outer products and adds them to Z");
|
||||
output ("for (j=i-$UNROLL1; j >= 0; j -= $UNROLL1) {\n");
|
||||
for ($k=0; $k < $UNROLL1; $k++) {
|
||||
innerOuterProductLoop ($rows,$k,$nrhs,($k==$UNROLL1-1) ? $UNROLL1 : 0);
|
||||
}
|
||||
|
||||
comment ("end of inner loop");
|
||||
output ("}\n");
|
||||
|
||||
if ($UNROLL1 > 1) {
|
||||
comment ("compute left-over iterations");
|
||||
output ("j += $UNROLL1;\n");
|
||||
output ("for (; j > 0; j--) {\n");
|
||||
innerOuterProductLoop ($rows,'0',$nrhs,1);
|
||||
output ("}\n");
|
||||
}
|
||||
|
||||
comment ("finish computing the X(i) block");
|
||||
|
||||
for ($j=1; $j<=$nrhs; $j++) {
|
||||
if ($TYPE eq 't') {
|
||||
output ("Z1$j = ex[".ofs1(-($j-1),'lskip')."] - Z1$j;\n");
|
||||
output ("ex[".ofs1(-($j-1),'lskip')."] = Z1$j;\n");
|
||||
}
|
||||
else {
|
||||
output ("Z1$j = ex[".ofs1($j-1,'lskip')."] - Z1$j;\n");
|
||||
output ("ex[".ofs1($j-1,'lskip')."] = Z1$j;\n");
|
||||
}
|
||||
}
|
||||
|
||||
for ($i=2; $i<=$rows; $i++) {
|
||||
for ($j=1; $j<$i; $j++) {
|
||||
if ($TYPE eq 't') {
|
||||
output ("p$j = ell[".ofs2(-($i-1),$j-1,'lskip')."];\n");
|
||||
}
|
||||
else {
|
||||
output ("p$j = ell[".ofs2($j-1,$i-1,'lskip')."];\n");
|
||||
}
|
||||
}
|
||||
for ($j=1; $j<=$nrhs; $j++) {
|
||||
if ($TYPE eq 't') {
|
||||
output ("Z$i$j = ex[".ofs2(-($i-1),$j-1,'lskip')."] - Z$i$j");
|
||||
}
|
||||
else {
|
||||
output ("Z$i$j = ex[".ofs2($i-1,$j-1,'lskip')."] - Z$i$j");
|
||||
}
|
||||
for ($k=1; $k < $i; $k++) {
|
||||
output (" - p$k*Z$k$j");
|
||||
}
|
||||
output (";\n");
|
||||
if ($TYPE eq 't') {
|
||||
output ("ex[".ofs2(-($i-1),$j-1,'lskip')."] = Z$i$j;\n");
|
||||
}
|
||||
else {
|
||||
output ("ex[".ofs2($i-1,$j-1,'lskip')."] = Z$i$j;\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
sub makeFastL1Solve # ( number-of-right-hand-sides )
|
||||
{
|
||||
my $nrhs = $_[0];
|
||||
my ($i,$j,$k);
|
||||
my $funcsuffix = ($TYPE eq 'f') ? "_$nrhs" : '';
|
||||
my $staticvoid = ($TYPE eq 'f') ? 'static void' : 'void';
|
||||
|
||||
# function header
|
||||
if ($TYPE eq 't') {
|
||||
output (<<END);
|
||||
|
||||
/* solve L^T * x=b, with b containing 1 right hand side.
|
||||
* L is an n*n lower triangular matrix with ones on the diagonal.
|
||||
* L is stored by rows and its leading dimension is lskip.
|
||||
* b is an n*1 matrix that contains the right hand side.
|
||||
* b is overwritten with x.
|
||||
* this processes blocks of $N1.
|
||||
*/
|
||||
|
||||
void dSolveL1T (const dReal *L, dReal *B, int n, int lskip1)
|
||||
{
|
||||
END
|
||||
}
|
||||
else {
|
||||
output (<<END);
|
||||
|
||||
/* solve L*X=B, with B containing $nrhs right hand sides.
|
||||
* L is an n*n lower triangular matrix with ones on the diagonal.
|
||||
* L is stored by rows and its leading dimension is lskip.
|
||||
* B is an n*$nrhs matrix that contains the right hand sides.
|
||||
* B is stored by columns and its leading dimension is also lskip.
|
||||
* B is overwritten with X.
|
||||
* this processes blocks of $N1*$N1.
|
||||
* if this is in the factorizer source file, n must be a multiple of $N1.
|
||||
*/
|
||||
|
||||
$staticvoid dSolveL1$funcsuffix (const dReal *L, dReal *B, int n, int lskip1)
|
||||
{
|
||||
END
|
||||
}
|
||||
|
||||
comment ("declare variables - Z matrix, p and q vectors, etc");
|
||||
output ("dReal ");
|
||||
for ($i=1; $i<=$N1; $i++) {
|
||||
for ($j=1; $j<=$nrhs; $j++) {
|
||||
output ("Z$i$j,"); # Z matrix
|
||||
output ("m$i$j,") if ! $MADD; # temporary vars where multiplies put
|
||||
}
|
||||
}
|
||||
for ($i=1; $i<=$N1; $i++) {
|
||||
output ("p$i,");
|
||||
output ("q$i,") if $i <= $nrhs;
|
||||
}
|
||||
output ("*ex;\nconst dReal *ell;\n");
|
||||
output ("int ");
|
||||
for ($i=2; $i<$N1; $i++) {
|
||||
output ("lskip$i,");
|
||||
}
|
||||
output ("i,j;\n");
|
||||
|
||||
if ($TYPE eq 't') {
|
||||
comment ("special handling for L and B because we're solving L1 *transpose*");
|
||||
output ("L = L + (n-1)*(lskip1+1);\n");
|
||||
output ("B = B + n-1;\n");
|
||||
output ("lskip1 = -lskip1;\n");
|
||||
}
|
||||
|
||||
if ($N1 > 2) {
|
||||
comment ("compute lskip values");
|
||||
for ($i=2; $i<$N1; $i++) {
|
||||
output ("lskip$i = $i*lskip1;\n");
|
||||
}
|
||||
}
|
||||
|
||||
comment ("compute all $N1 x $nrhs blocks of X");
|
||||
if ($TYPE eq 's' or $TYPE eq 't') {
|
||||
output ("for (i=0; i <= n-$N1; i+=$N1) {\n");
|
||||
}
|
||||
else {
|
||||
output ("for (i=0; i < n; i+=$N1) {\n");
|
||||
}
|
||||
computeRows ($nrhs,$N1);
|
||||
comment ("end of outer loop");
|
||||
output ("}\n");
|
||||
|
||||
if ($TYPE eq 's' or $TYPE eq 't') {
|
||||
comment ("compute rows at end that are not a multiple of block size");
|
||||
output ("for (; i < n; i++) {\n");
|
||||
computeRows ($nrhs,1);
|
||||
output ("}\n");
|
||||
}
|
||||
|
||||
output ("}\n");
|
||||
}
|
||||
|
||||
#############################################################################
|
||||
# make a fast L*D*L' factorizer
|
||||
|
||||
# code fragment: this factors an M x M block. if A_or_Z is 0 then it works
|
||||
# on the $A matrix otherwise it works on the Z matrix. in either case it
|
||||
# writes the diagonal entries into the `dee' vector.
|
||||
# it is a simple implementation of the LDLT algorithm, with no tricks.
|
||||
|
||||
sub getA # (i,j,A,A_or_Z)
|
||||
{
|
||||
my $i = $_[0];
|
||||
my $j = $_[1];
|
||||
my $A = $_[2];
|
||||
return $_[3] ? ('Z'.($i+1).($j+1)) : ($A.'['.ofs2($j,$i,'nskip').']');
|
||||
}
|
||||
|
||||
sub miniLDLT # (A,A_or_Z,M)
|
||||
{
|
||||
my ($i,$j,$k);
|
||||
my $A = $_[0];
|
||||
my $AZ = $_[1];
|
||||
my $M = $_[2];
|
||||
comment ("factorize $M x $M block " . ($AZ ? "Z,dee" : "$A,dee"));
|
||||
comment ("factorize row 1");
|
||||
output ("dee[0] = dRecip(".getA(0,0,$A,$AZ).");\n");
|
||||
for ($i=1; $i<$M; $i++) {
|
||||
comment ("factorize row ".($i+1));
|
||||
for ($j=1; $j<$i; $j++) {
|
||||
output (getA($i,$j,$A,$AZ)." -= ");
|
||||
for ($k=0; $k<$j; $k++) {
|
||||
output (" + ") if $k > 0;
|
||||
output (getA($i,$k,$A,$AZ)."*".getA($j,$k,$A,$AZ));
|
||||
}
|
||||
output (";\n");
|
||||
}
|
||||
output ("sum = 0;\n");
|
||||
for ($j=0; $j<$i; $j++) {
|
||||
output ("q1 = ".getA($i,$j,$A,$AZ).";\n");
|
||||
output ("q2 = q1 * dee[$j];\n");
|
||||
output (getA($i,$j,$A,$AZ)." = q2;\n");
|
||||
output ("sum += q1*q2;\n");
|
||||
}
|
||||
output ("dee[$i] = dRecip(".getA($i,$i,$A,$AZ)." - sum);\n");
|
||||
}
|
||||
comment ("done factorizing $M x $M block");
|
||||
}
|
||||
|
||||
|
||||
sub innerScaleAndOuterProductLoop # (M,k)
|
||||
{
|
||||
my $M = $_[0];
|
||||
my $k = $_[1];
|
||||
my ($i,$j);
|
||||
for ($i=1; $i<=$M; $i++) {
|
||||
output ("p$i = ell[".ofs2($k,$i-1,'nskip')."];\n");
|
||||
}
|
||||
output ("dd = dee[$k];\n");
|
||||
for ($i=1; $i<=$M; $i++) {
|
||||
output ("q$i = p$i*dd;\n");
|
||||
}
|
||||
for ($i=1; $i<=$M; $i++) {
|
||||
output ("ell[".ofs2($k,$i-1,'nskip')."] = q$i;\n");
|
||||
}
|
||||
for ($i=1; $i<=$M; $i++) {
|
||||
for ($j=1; $j<=$i; $j++) {
|
||||
my $var = $MADD ? "Z$i$j +=" : "m$i$j =";
|
||||
output ("$var p$i*q$j;\n");
|
||||
}
|
||||
}
|
||||
if ($MADD==0) {
|
||||
for ($i=1; $i<=$M; $i++) {
|
||||
for ($j=1; $j<=$i; $j++) {
|
||||
output ("Z$i$j += m$i$j;\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
sub diagRows # (M)
|
||||
{
|
||||
my $M=$_[0];
|
||||
comment ("scale the elements in a $M x i block at A(i,0), and also");
|
||||
comment ("compute Z = the outer product matrix that we'll need.");
|
||||
for ($i=1; $i<=$M; $i++) {
|
||||
for ($j=1; $j<=$i; $j++) {
|
||||
output ("Z$i$j = 0;\n");
|
||||
}
|
||||
}
|
||||
output ("ell = A+i*nskip1;\n");
|
||||
output ("dee = d;\n");
|
||||
output ("for (j=i-$UNROLL2; j >= 0; j -= $UNROLL2) {\n");
|
||||
for ($i=0; $i < $UNROLL2; $i++) {
|
||||
innerScaleAndOuterProductLoop ($M,$i);
|
||||
}
|
||||
output ("ell += $UNROLL2;\n");
|
||||
output ("dee += $UNROLL2;\n");
|
||||
output ("}\n");
|
||||
|
||||
if ($UNROLL2 > 1) {
|
||||
comment ("compute left-over iterations");
|
||||
output ("j += $UNROLL2;\n");
|
||||
output ("for (; j > 0; j--) {\n");
|
||||
innerScaleAndOuterProductLoop ($M,0);
|
||||
output ("ell++;\n");
|
||||
output ("dee++;\n");
|
||||
output ("}\n");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
sub diagBlock # (M)
|
||||
{
|
||||
my $M = $_[0];
|
||||
comment ("solve for diagonal $M x $M block at A(i,i)");
|
||||
for ($i=1; $i<=$M; $i++) {
|
||||
for ($j=1; $j<=$i; $j++) {
|
||||
output ("Z$i$j = ell[".ofs2($j-1,$i-1,'nskip')."] - Z$i$j;\n");
|
||||
}
|
||||
}
|
||||
output ("dee = d + i;\n");
|
||||
miniLDLT ('',1,$M);
|
||||
for ($i=2; $i<=$M; $i++) {
|
||||
for ($j=1; $j<$i; $j++) {
|
||||
output ("ell[".ofs2($j-1,$i-1,'nskip')."] = Z$i$j;\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
sub makeFastLDLT
|
||||
{
|
||||
my ($i,$j,$k);
|
||||
|
||||
# function header
|
||||
output (<<END);
|
||||
|
||||
|
||||
void dFactorLDLT (dReal *A, dReal *d, int n, int nskip1)
|
||||
{
|
||||
END
|
||||
output ("int i,j");
|
||||
for ($i=2; $i<$N1; $i++) {
|
||||
output (",nskip$i");
|
||||
}
|
||||
output (";\n");
|
||||
output ("dReal sum,*ell,*dee,dd,p1,p2");
|
||||
for ($i=3; $i<=$N1; $i++) {
|
||||
output (",p$i");
|
||||
}
|
||||
for ($i=1; $i<=$N1; $i++) {
|
||||
output (",q$i");
|
||||
}
|
||||
for ($i=1; $i<=$N1; $i++) {
|
||||
for ($j=1; $j<=$i; $j++) {
|
||||
output (",Z$i$j");
|
||||
output (",m$i$j") if ! $MADD; # temporary vars where multiplies put
|
||||
}
|
||||
}
|
||||
output (";\n");
|
||||
output ("if (n < 1) return;\n");
|
||||
# output ("nskip1 = dPAD(n);\n"); ... not any more
|
||||
for ($i=2; $i<$N1; $i++) {
|
||||
output ("nskip$i = $i*nskip1;\n");
|
||||
}
|
||||
|
||||
output ("\nfor (i=0; i<=n-$N1; i += $N1) {\n");
|
||||
comment ("solve L*(D*l)=a, l is scaled elements in $N1 x i block at A(i,0)");
|
||||
output ("dSolveL1_$N1 (A,A+i*nskip1,i,nskip1);\n");
|
||||
|
||||
diagRows ($N1);
|
||||
diagBlock ($N1);
|
||||
output ("}\n");
|
||||
|
||||
comment ("compute the (less than $N1) rows at the bottom");
|
||||
output ("switch (n-i) {\n");
|
||||
output ("case 0:\n");
|
||||
output ("break;\n\n");
|
||||
|
||||
for ($i=1; $i<$N1; $i++) {
|
||||
output ("case $i:\n");
|
||||
output ("dSolveL1_$i (A,A+i*nskip1,i,nskip1);\n");
|
||||
diagRows ($i);
|
||||
diagBlock ($i);
|
||||
output ("break;\n\n");
|
||||
}
|
||||
|
||||
output ("default: *((char*)0)=0; /* this should never happen! */\n");
|
||||
output ("}\n");
|
||||
|
||||
output ("}\n");
|
||||
}
|
||||
|
||||
#############################################################################
|
||||
# write source code
|
||||
|
||||
open (FOUT,">$FNAME.c") or die "can't open $FNAME.c for writing";
|
||||
|
||||
# file and function header
|
||||
output (<<END);
|
||||
/* generated code, do not edit. */
|
||||
|
||||
#include "ode/matrix.h"
|
||||
END
|
||||
|
||||
if ($TYPE eq 'f') {
|
||||
for ($i=1; $i <= $N1; $i++) {
|
||||
makeFastL1Solve ($i);
|
||||
}
|
||||
makeFastLDLT;
|
||||
}
|
||||
else {
|
||||
makeFastL1Solve (1);
|
||||
makeRealFastL1Solve;
|
||||
}
|
||||
close FOUT;
|
||||
174
extern/ode/dist/ode/fbuild/BuildMultidot
vendored
174
extern/ode/dist/ode/fbuild/BuildMultidot
vendored
@@ -1,174 +0,0 @@
|
||||
#!/usr/bin/perl
|
||||
#
|
||||
#########################################################################
|
||||
# #
|
||||
# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. #
|
||||
# All rights reserved. Email: russ@q12.org Web: www.q12.org #
|
||||
# #
|
||||
# This library is free software; you can redistribute it and/or #
|
||||
# modify it under the terms of EITHER: #
|
||||
# (1) The GNU Lesser General Public License as published by the Free #
|
||||
# Software Foundation; either version 2.1 of the License, or (at #
|
||||
# your option) any later version. The text of the GNU Lesser #
|
||||
# General Public License is included with this library in the #
|
||||
# file LICENSE.TXT. #
|
||||
# (2) The BSD-style license that is included with this library in #
|
||||
# the file LICENSE-BSD.TXT. #
|
||||
# #
|
||||
# This library is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files #
|
||||
# LICENSE.TXT and LICENSE-BSD.TXT for more details. #
|
||||
# #
|
||||
#########################################################################
|
||||
|
||||
# multi-dot-product code generator. this code generator is based on the
|
||||
# dot-product code generator.
|
||||
#
|
||||
# code generation parameters, set in a parameters file:
|
||||
# FNAME : name of source file to generate - a .c file will be made
|
||||
# N1 : block size (number of `a' vectors to dot)
|
||||
# UNROLL1 : inner loop unrolling factor (1..)
|
||||
# FETCH : max num of a[i]'s and b[i]'s to load ahead of muls
|
||||
# LAT1 : load -> mul latency (>=1)
|
||||
# LAT2 : mul -> add latency (>=1). if this is 1, use fused mul-add
|
||||
#
|
||||
#############################################################################
|
||||
|
||||
require ("BuildUtil");
|
||||
|
||||
# get and check code generation parameters
|
||||
error ("Usage: BuildMultidot <parameters-file>") if $#ARGV != 0;
|
||||
do $ARGV[0];
|
||||
|
||||
if (!defined($FNAME) || !defined($N1) || !defined($UNROLL1) ||
|
||||
!defined($FETCH) || !defined($LAT1) || !defined($LAT2)) {
|
||||
error ("code generation parameters not defined");
|
||||
}
|
||||
|
||||
# check parameters
|
||||
error ("bad N1") if $N1 < 2;
|
||||
error ("bad UNROLL1") if $UNROLL1 < 1;
|
||||
error ("bad FETCH") if $FETCH < 1;
|
||||
error ("bad LAT1") if $LAT1 < 1;
|
||||
error ("bad LAT2") if $LAT2 < 1;
|
||||
|
||||
#############################################################################
|
||||
|
||||
open (FOUT,">$FNAME.c") or die "can't open $FNAME.c for writing";
|
||||
|
||||
# file and function header
|
||||
output (<<END);
|
||||
/* generated code, do not edit. */
|
||||
|
||||
#include "ode/matrix.h"
|
||||
|
||||
|
||||
END
|
||||
output ("void dMultidot$N1 (");
|
||||
for ($i=0; $i<$N1; $i++) {
|
||||
output ("const dReal *a$i, ");
|
||||
}
|
||||
output ("const dReal *b, dReal *outsum, int n)\n{\n");
|
||||
|
||||
output ("dReal ");
|
||||
for ($i=0; $i<$UNROLL1; $i++) {
|
||||
for ($j=0; $j<$N1; $j++) {
|
||||
output ("p$i$j,");
|
||||
output ("m$i$j,") if $LAT2 > 1;
|
||||
}
|
||||
output ("q$i,");
|
||||
}
|
||||
for ($i=0; $i<$N1; $i++) {
|
||||
output ("sum$i");
|
||||
output (",") if $i < ($N1-1);
|
||||
}
|
||||
output (";\n");
|
||||
for ($i=0; $i<$N1; $i++) {
|
||||
output ("sum$i = 0;\n");
|
||||
}
|
||||
output (<<END);
|
||||
n -= $UNROLL1;
|
||||
while (n >= 0) {
|
||||
END
|
||||
|
||||
@load = (); # slot where a[i]'s and b[i]'s loaded
|
||||
@mul = (); # slot where multiply i happened
|
||||
@add = (); # slow where add i happened
|
||||
|
||||
for ($i=0; $i<$N1; $i++) {
|
||||
output ("p0$i = a$i [0];\n");
|
||||
}
|
||||
output ("q0 = b[0];\n");
|
||||
push (@load,0);
|
||||
|
||||
$slot=0; # one slot for every load/mul/add/nop issued
|
||||
for (;;) {
|
||||
$startslot = $slot;
|
||||
|
||||
# do next load
|
||||
if (($#load - $#mul) < $FETCH && ($#load+1) < $UNROLL1) {
|
||||
push (@load,$slot);
|
||||
for ($j=0; $j<$N1; $j++) {
|
||||
output ("p$#load$j = a$j [$#load];\n");
|
||||
}
|
||||
output ("q$#load = b[$#load];\n");
|
||||
$slot++;
|
||||
}
|
||||
|
||||
# do next multiply
|
||||
if ($#load > $#mul && $slot >= ($load[$#mul+1] + $LAT1) &&
|
||||
($#mul+1) < $UNROLL1) {
|
||||
push (@mul,$slot);
|
||||
if ($LAT2 > 1) {
|
||||
for ($j=0; $j<$N1; $j++) {
|
||||
output ("m$#mul$j = p$#mul$j * q$#mul;\n");
|
||||
}
|
||||
}
|
||||
else {
|
||||
for ($j=0; $j<$N1; $j++) {
|
||||
output ("sum$j += p$#mul$j * q$#mul;\n");
|
||||
}
|
||||
last if ($#mul+1) >= $UNROLL1;
|
||||
}
|
||||
$slot++;
|
||||
}
|
||||
# do next add
|
||||
if ($LAT2 > 1) {
|
||||
if ($#mul > $#add && $slot >= ($mul[$#add+1] + $LAT2)) {
|
||||
push (@add,$slot);
|
||||
for ($j=0; $j<$N1; $j++) {
|
||||
output ("sum$j += m$#add$j;\n");
|
||||
}
|
||||
$slot++;
|
||||
last if ($#add+1) >= $UNROLL1;
|
||||
}
|
||||
}
|
||||
|
||||
if ($slot == $startslot) {
|
||||
# comment ("nop");
|
||||
$slot++;
|
||||
}
|
||||
}
|
||||
|
||||
for ($j=0; $j<$N1; $j++) {
|
||||
output ("a$j += $UNROLL1;\n");
|
||||
}
|
||||
output ("b += $UNROLL1;\n");
|
||||
output ("n -= $UNROLL1;\n");
|
||||
output ("}\n");
|
||||
|
||||
output ("n += $UNROLL1;\n");
|
||||
output ("while (n > 0) {\n");
|
||||
output ("q0 = *b;\n");
|
||||
for ($j=0; $j<$N1; $j++) {
|
||||
output ("sum$j += (*a$j) * q0;\n");
|
||||
output ("a$j++;\n");
|
||||
}
|
||||
output ("b++;\n");
|
||||
output ("n--;\n");
|
||||
output ("}\n");
|
||||
for ($j=0; $j<$N1; $j++) {
|
||||
output ("outsum[$j] = sum$j;\n");
|
||||
}
|
||||
output ("}\n");
|
||||
99
extern/ode/dist/ode/fbuild/BuildUtil
vendored
99
extern/ode/dist/ode/fbuild/BuildUtil
vendored
@@ -1,99 +0,0 @@
|
||||
#!/usr/bin/perl -w
|
||||
#
|
||||
#########################################################################
|
||||
# #
|
||||
# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. #
|
||||
# All rights reserved. Email: russ@q12.org Web: www.q12.org #
|
||||
# #
|
||||
# This library is free software; you can redistribute it and/or #
|
||||
# modify it under the terms of EITHER: #
|
||||
# (1) The GNU Lesser General Public License as published by the Free #
|
||||
# Software Foundation; either version 2.1 of the License, or (at #
|
||||
# your option) any later version. The text of the GNU Lesser #
|
||||
# General Public License is included with this library in the #
|
||||
# file LICENSE.TXT. #
|
||||
# (2) The BSD-style license that is included with this library in #
|
||||
# the file LICENSE-BSD.TXT. #
|
||||
# #
|
||||
# This library is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files #
|
||||
# LICENSE.TXT and LICENSE-BSD.TXT for more details. #
|
||||
# #
|
||||
#########################################################################
|
||||
|
||||
package BuildUtil;
|
||||
|
||||
|
||||
# print out code. after newlines, indent according to the number of curly
|
||||
# brackets we've seen
|
||||
|
||||
my $indent = 0;
|
||||
my $startofline = 1;
|
||||
|
||||
|
||||
sub main::output
|
||||
{
|
||||
my $line = $_[0];
|
||||
my ($i,$j,$c);
|
||||
for ($i=0; $i < length ($line); $i++) {
|
||||
$c = substr ($line,$i,1);
|
||||
print main::FOUT $c if $c eq '{';
|
||||
$indent++ if $c eq '{';
|
||||
$indent-- if $c eq '}';
|
||||
if ($startofline) {
|
||||
for ($j=0; $j < $indent; $j++) {
|
||||
print main::FOUT " ";
|
||||
}
|
||||
$startofline = 0;
|
||||
}
|
||||
print main::FOUT $c if $c ne '{';
|
||||
$startofline = 1 if $c eq "\n";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
# write a C comment with the correct indenting
|
||||
|
||||
sub main::comment
|
||||
{
|
||||
main::output ("/* $_[0] */\n");
|
||||
}
|
||||
|
||||
|
||||
# return an offset: N*skip = skipN where N=0,1,2,...
|
||||
|
||||
sub main::ofs1 # (N,skip)
|
||||
{
|
||||
my $N = $_[0];
|
||||
my $skip = $_[1];
|
||||
return '0' if $N==0;
|
||||
return $skip . $N;
|
||||
}
|
||||
|
||||
|
||||
# return an offset: M+N*skip = M+skipN where N=0,1,2,...
|
||||
|
||||
sub main::ofs2 # (M,N,skip)
|
||||
{
|
||||
my $M = $_[0];
|
||||
my $N = $_[1];
|
||||
my $skip = $_[2];
|
||||
$M = '0' if $M eq '-0';
|
||||
my $a = $M;
|
||||
$a .= '+' . $skip . $N if ($N > 0);
|
||||
substr ($a,0,2)='' if substr ($a,0,2) eq '0+';
|
||||
return $a;
|
||||
}
|
||||
|
||||
|
||||
# print an error message and exit
|
||||
|
||||
sub main::error
|
||||
{
|
||||
print "ERROR: $_[0]\n";
|
||||
exit 1;
|
||||
}
|
||||
|
||||
|
||||
1;
|
||||
16
extern/ode/dist/ode/fbuild/Dependencies
vendored
16
extern/ode/dist/ode/fbuild/Dependencies
vendored
@@ -1,16 +0,0 @@
|
||||
test_dot.o: test_dot.cpp ../../include/ode/ode.h \
|
||||
../../include/ode/config.h ../../include/ode/contact.h \
|
||||
../../include/ode/common.h ../../include/ode/error.h \
|
||||
../../include/ode/memory.h ../../include/ode/odemath.h \
|
||||
../../include/ode/matrix.h ../../include/ode/timer.h \
|
||||
../../include/ode/rotation.h ../../include/ode/mass.h \
|
||||
../../include/ode/space.h ../../include/ode/geom.h \
|
||||
../../include/ode/misc.h
|
||||
test_ldlt.o: test_ldlt.cpp ../../include/ode/ode.h \
|
||||
../../include/ode/config.h ../../include/ode/contact.h \
|
||||
../../include/ode/common.h ../../include/ode/error.h \
|
||||
../../include/ode/memory.h ../../include/ode/odemath.h \
|
||||
../../include/ode/matrix.h ../../include/ode/timer.h \
|
||||
../../include/ode/rotation.h ../../include/ode/mass.h \
|
||||
../../include/ode/space.h ../../include/ode/geom.h \
|
||||
../../include/ode/misc.h
|
||||
77
extern/ode/dist/ode/fbuild/Makefile
vendored
77
extern/ode/dist/ode/fbuild/Makefile
vendored
@@ -1,77 +0,0 @@
|
||||
#########################################################################
|
||||
# #
|
||||
# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. #
|
||||
# All rights reserved. Email: russ@q12.org Web: www.q12.org #
|
||||
# #
|
||||
# This library is free software; you can redistribute it and/or #
|
||||
# modify it under the terms of EITHER: #
|
||||
# (1) The GNU Lesser General Public License as published by the Free #
|
||||
# Software Foundation; either version 2.1 of the License, or (at #
|
||||
# your option) any later version. The text of the GNU Lesser #
|
||||
# General Public License is included with this library in the #
|
||||
# file LICENSE.TXT. #
|
||||
# (2) The BSD-style license that is included with this library in #
|
||||
# the file LICENSE-BSD.TXT. #
|
||||
# #
|
||||
# This library is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files #
|
||||
# LICENSE.TXT and LICENSE-BSD.TXT for more details. #
|
||||
# #
|
||||
#########################################################################
|
||||
|
||||
# currently this only works under linux, and it's a bit of a mess!
|
||||
|
||||
MAKEFILE_INC=../../build/Makefile.inc
|
||||
include $(MAKEFILE_INC)
|
||||
|
||||
INCLUDE_PATHS=../../include
|
||||
LIB_PATHS = ../../lib
|
||||
DEFINES=dDOUBLE
|
||||
|
||||
SOURCES_CPP=test_ldlt.cpp
|
||||
SOURCES_C=fastldlt.c fastlsolve.c fastltsolve.c
|
||||
APPS=$(call fEXENAME,test_ldlt) $(call fEXENAME,test_dot) $(call fEXENAME,test_multidot)
|
||||
EXTRA_CLEAN=test_ldlt test_dot test_multidot fastldlt.c fastlsolve.c fastltsolve.c fastdot.c fastmultidot.c
|
||||
|
||||
|
||||
all: $(APPS)
|
||||
|
||||
$(call fEXENAME,test_ldlt): $(call fTARGETS,$(SOURCES_CPP) $(SOURCES_C))
|
||||
gcc -o $@ $^ -L $(LIB_PATHS) $(call fLIB,ode) -lm
|
||||
|
||||
$(call fEXENAME,test_dot): test_dot.o fastdot.o
|
||||
gcc -o $@ test_dot.o fastdot.o -L $(LIB_PATHS) $(call fLIB,ode) -lm
|
||||
|
||||
$(call fEXENAME,test_multidot): test_multidot.o fastmultidot.o
|
||||
gcc -o $@ test_multidot.o fastmultidot.o -L $(LIB_PATHS) $(call fLIB,ode) -lm
|
||||
|
||||
fastldlt.o: fastldlt.c
|
||||
gcc -O1 -I$(INCLUDE_PATHS) -ffast-math -fomit-frame-pointer -c -D$(DEFINES) $<
|
||||
|
||||
fastlsolve.o: fastlsolve.c
|
||||
gcc -O1 -I$(INCLUDE_PATHS) -ffast-math -fomit-frame-pointer -c -D$(DEFINES) $<
|
||||
|
||||
fastltsolve.o: fastltsolve.c
|
||||
gcc -O1 -I$(INCLUDE_PATHS) -ffast-math -fomit-frame-pointer -c -D$(DEFINES) $<
|
||||
|
||||
fastdot.o: fastdot.c
|
||||
gcc -O1 -I$(INCLUDE_PATHS) -ffast-math -fomit-frame-pointer -c -D$(DEFINES) $<
|
||||
|
||||
fastmultidot.o: fastmultidot.c
|
||||
gcc -O1 -I$(INCLUDE_PATHS) -ffast-math -fomit-frame-pointer -c -D$(DEFINES) $<
|
||||
|
||||
fastldlt.c: BuildLDLT BuildUtil ParametersF
|
||||
./BuildLDLT ParametersF
|
||||
|
||||
fastlsolve.c: BuildLDLT BuildUtil ParametersS
|
||||
./BuildLDLT ParametersS
|
||||
|
||||
fastltsolve.c: BuildLDLT BuildUtil ParametersT
|
||||
./BuildLDLT ParametersT
|
||||
|
||||
fastdot.c: BuildDot BuildUtil ParametersD
|
||||
./BuildDot ParametersD
|
||||
|
||||
fastmultidot.c: BuildMultidot BuildUtil ParametersM
|
||||
./BuildMultidot ParametersM
|
||||
71
extern/ode/dist/ode/fbuild/OptimizeDot
vendored
71
extern/ode/dist/ode/fbuild/OptimizeDot
vendored
@@ -1,71 +0,0 @@
|
||||
#!/usr/bin/perl
|
||||
#
|
||||
#########################################################################
|
||||
# #
|
||||
# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. #
|
||||
# All rights reserved. Email: russ@q12.org Web: www.q12.org #
|
||||
# #
|
||||
# This library is free software; you can redistribute it and/or #
|
||||
# modify it under the terms of EITHER: #
|
||||
# (1) The GNU Lesser General Public License as published by the Free #
|
||||
# Software Foundation; either version 2.1 of the License, or (at #
|
||||
# your option) any later version. The text of the GNU Lesser #
|
||||
# General Public License is included with this library in the #
|
||||
# file LICENSE.TXT. #
|
||||
# (2) The BSD-style license that is included with this library in #
|
||||
# the file LICENSE-BSD.TXT. #
|
||||
# #
|
||||
# This library is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files #
|
||||
# LICENSE.TXT and LICENSE-BSD.TXT for more details. #
|
||||
# #
|
||||
#########################################################################
|
||||
|
||||
# optimize the dot product built by BuildDot
|
||||
|
||||
##############################################################################
|
||||
|
||||
require ("OptimizeUtil");
|
||||
|
||||
# unused standard parameters
|
||||
$TYPE='unused';
|
||||
$N1=0; # unused
|
||||
$UNROLL2=0; # unused
|
||||
$MADD=0; # unused
|
||||
|
||||
##############################################################################
|
||||
|
||||
sub testDot # (filename)
|
||||
{
|
||||
my $filename = $_[0];
|
||||
createParametersFile ('ParametersD');
|
||||
$params = "$N1 $UNROLL1 $UNROLL2 $MADD $FETCH $LAT1 $LAT2";
|
||||
print "***** TESTING $params\n";
|
||||
doit ("rm -f fastdot.c fastdot.o test_dot");
|
||||
doit ("make test_dot");
|
||||
doit ("./test_dot >> $filename");
|
||||
open (FILE,">>$filename");
|
||||
print FILE " $params\n";
|
||||
close FILE;
|
||||
}
|
||||
|
||||
# find optimal parameters. write results to data4.txt
|
||||
|
||||
open (FILE,">data4.txt");
|
||||
print FILE "# dot product data from OptimizeDot\n";
|
||||
close FILE;
|
||||
$FNAME='fastdot';
|
||||
|
||||
for ($UNROLL1=1; $UNROLL1 <= 10; $UNROLL1++) {
|
||||
for ($LAT1=1; $LAT1 <= 5; $LAT1++) {
|
||||
for ($LAT2=1; $LAT2 <= 5; $LAT2++) {
|
||||
for ($FETCH=1; $FETCH<=5; $FETCH++) {
|
||||
testDot ('data4.txt');
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
readBackDataFile ('data4.txt');
|
||||
createParametersFile ('ParametersD');
|
||||
91
extern/ode/dist/ode/fbuild/OptimizeLDLT
vendored
91
extern/ode/dist/ode/fbuild/OptimizeLDLT
vendored
@@ -1,91 +0,0 @@
|
||||
#!/usr/bin/perl
|
||||
#
|
||||
#########################################################################
|
||||
# #
|
||||
# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. #
|
||||
# All rights reserved. Email: russ@q12.org Web: www.q12.org #
|
||||
# #
|
||||
# This library is free software; you can redistribute it and/or #
|
||||
# modify it under the terms of EITHER: #
|
||||
# (1) The GNU Lesser General Public License as published by the Free #
|
||||
# Software Foundation; either version 2.1 of the License, or (at #
|
||||
# your option) any later version. The text of the GNU Lesser #
|
||||
# General Public License is included with this library in the #
|
||||
# file LICENSE.TXT. #
|
||||
# (2) The BSD-style license that is included with this library in #
|
||||
# the file LICENSE-BSD.TXT. #
|
||||
# #
|
||||
# This library is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files #
|
||||
# LICENSE.TXT and LICENSE-BSD.TXT for more details. #
|
||||
# #
|
||||
#########################################################################
|
||||
|
||||
# optimize the factorizer built by BuildLDLT
|
||||
#
|
||||
# FNAME : name of source file to generate - .h and .c files will be made
|
||||
# N1 : block size (size of outer product matrix) (1..9)
|
||||
# UNROLL1 : solver inner loop unrolling factor (1..)
|
||||
# UNROLL2 : factorizer inner loop unrolling factor (1..)
|
||||
# MADD : if nonzero, generate code for fused multiply-add (0,1)
|
||||
# FETCH : how to fetch data in the inner loop:
|
||||
# 0 - load in a batch (the `normal way')
|
||||
# 1 - delay inner loop loads until just before they're needed
|
||||
|
||||
##############################################################################
|
||||
|
||||
require ("OptimizeUtil");
|
||||
|
||||
##############################################################################
|
||||
# optimize factorizer
|
||||
|
||||
sub testFactorizer # (filename)
|
||||
{
|
||||
my $filename = $_[0];
|
||||
createParametersFile ('ParametersF');
|
||||
$params = "$N1 $UNROLL1 $UNROLL2 $MADD $FETCH";
|
||||
print "***** TESTING $params\n";
|
||||
doit ("rm -f fastldlt.c fastldlt.o test_ldlt");
|
||||
doit ("make test_ldlt");
|
||||
doit ("./test_ldlt f >> $filename");
|
||||
open (FILE,">>$filename");
|
||||
print FILE " $params\n";
|
||||
close FILE;
|
||||
}
|
||||
|
||||
|
||||
# first find optimal parameters ignoring UNROLL1 and UNROLL2, write results
|
||||
# to data1.txt
|
||||
|
||||
open (FILE,">data1.txt");
|
||||
print FILE "# factorizer data from OptimizeLDLT\n";
|
||||
close FILE;
|
||||
$FNAME='fastldlt';
|
||||
$TYPE='f';
|
||||
$UNROLL1=4;
|
||||
$UNROLL2=4;
|
||||
for ($N1=1; $N1 <= 4; $N1++) {
|
||||
for ($MADD=0; $MADD<=1; $MADD++) {
|
||||
for ($FETCH=0; $FETCH<=1; $FETCH++) {
|
||||
testFactorizer ('data1.txt');
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
readBackDataFile ('data1.txt');
|
||||
createParametersFile ('ParametersF');
|
||||
|
||||
# now find optimal UNROLL1 and UNROLL2 values, write results to data2.txt
|
||||
|
||||
open (FILE,">data2.txt");
|
||||
print FILE "# factorizer data from OptimizeLDLT\n";
|
||||
close FILE;
|
||||
for ($UNROLL1=1; $UNROLL1 <= 10; $UNROLL1++) {
|
||||
for ($UNROLL2=1; $UNROLL2 <= 10; $UNROLL2++) {
|
||||
testFactorizer ('data2.txt');
|
||||
}
|
||||
}
|
||||
|
||||
readBackDataFile ('data2.txt');
|
||||
createParametersFile ('ParametersF');
|
||||
76
extern/ode/dist/ode/fbuild/OptimizeLSolve
vendored
76
extern/ode/dist/ode/fbuild/OptimizeLSolve
vendored
@@ -1,76 +0,0 @@
|
||||
#!/usr/bin/perl
|
||||
#
|
||||
#########################################################################
|
||||
# #
|
||||
# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. #
|
||||
# All rights reserved. Email: russ@q12.org Web: www.q12.org #
|
||||
# #
|
||||
# This library is free software; you can redistribute it and/or #
|
||||
# modify it under the terms of EITHER: #
|
||||
# (1) The GNU Lesser General Public License as published by the Free #
|
||||
# Software Foundation; either version 2.1 of the License, or (at #
|
||||
# your option) any later version. The text of the GNU Lesser #
|
||||
# General Public License is included with this library in the #
|
||||
# file LICENSE.TXT. #
|
||||
# (2) The BSD-style license that is included with this library in #
|
||||
# the file LICENSE-BSD.TXT. #
|
||||
# #
|
||||
# This library is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files #
|
||||
# LICENSE.TXT and LICENSE-BSD.TXT for more details. #
|
||||
# #
|
||||
#########################################################################
|
||||
|
||||
# optimize the solver built by BuildLDLT
|
||||
#
|
||||
# FNAME : name of source file to generate - .h and .c files will be made
|
||||
# N1 : block size (size of outer product matrix) (1..9)
|
||||
# UNROLL1 : solver inner loop unrolling factor (1..)
|
||||
# UNROLL2 : factorizer inner loop unrolling factor (1..)
|
||||
# MADD : if nonzero, generate code for fused multiply-add (0,1)
|
||||
# FETCH : how to fetch data in the inner loop:
|
||||
# 0 - load in a batch (the `normal way')
|
||||
# 1 - delay inner loop loads until just before they're needed
|
||||
|
||||
##############################################################################
|
||||
|
||||
require ("OptimizeUtil");
|
||||
|
||||
##############################################################################
|
||||
# optimize solver
|
||||
|
||||
sub testSolver # (filename)
|
||||
{
|
||||
my $filename = $_[0];
|
||||
createParametersFile ('ParametersS');
|
||||
$params = "$N1 $UNROLL1 $UNROLL2 $MADD $FETCH";
|
||||
print "***** TESTING $params\n";
|
||||
doit ("rm -f fastlsolve.c fastlsolve.o test_ldlt");
|
||||
doit ("make test_ldlt");
|
||||
doit ("./test_ldlt s >> $filename");
|
||||
open (FILE,">>$filename");
|
||||
print FILE " $params\n";
|
||||
close FILE;
|
||||
}
|
||||
|
||||
# find optimal parameters. UNROLL2 has no effect. write results to data3.txt
|
||||
|
||||
open (FILE,">data3.txt");
|
||||
print FILE "# solver data from OptimizeLDLT\n";
|
||||
close FILE;
|
||||
$FNAME='fastlsolve';
|
||||
$TYPE='s';
|
||||
$UNROLL2=1;
|
||||
for ($N1=1; $N1 <= 5; $N1++) {
|
||||
for ($UNROLL1=1; $UNROLL1 <= 15; $UNROLL1++) {
|
||||
for ($MADD=0; $MADD<=1; $MADD++) {
|
||||
for ($FETCH=0; $FETCH<=1; $FETCH++) {
|
||||
testSolver ('data3.txt');
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
readBackDataFile ('data3.txt');
|
||||
createParametersFile ('ParametersS');
|
||||
76
extern/ode/dist/ode/fbuild/OptimizeLTSolve
vendored
76
extern/ode/dist/ode/fbuild/OptimizeLTSolve
vendored
@@ -1,76 +0,0 @@
|
||||
#!/usr/bin/perl
|
||||
#
|
||||
#########################################################################
|
||||
# #
|
||||
# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. #
|
||||
# All rights reserved. Email: russ@q12.org Web: www.q12.org #
|
||||
# #
|
||||
# This library is free software; you can redistribute it and/or #
|
||||
# modify it under the terms of EITHER: #
|
||||
# (1) The GNU Lesser General Public License as published by the Free #
|
||||
# Software Foundation; either version 2.1 of the License, or (at #
|
||||
# your option) any later version. The text of the GNU Lesser #
|
||||
# General Public License is included with this library in the #
|
||||
# file LICENSE.TXT. #
|
||||
# (2) The BSD-style license that is included with this library in #
|
||||
# the file LICENSE-BSD.TXT. #
|
||||
# #
|
||||
# This library is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files #
|
||||
# LICENSE.TXT and LICENSE-BSD.TXT for more details. #
|
||||
# #
|
||||
#########################################################################
|
||||
|
||||
# optimize the transpose solver built by BuildLDLT
|
||||
#
|
||||
# FNAME : name of source file to generate - .h and .c files will be made
|
||||
# N1 : block size (size of outer product matrix) (1..9)
|
||||
# UNROLL1 : solver inner loop unrolling factor (1..)
|
||||
# UNROLL2 : factorizer inner loop unrolling factor (1..)
|
||||
# MADD : if nonzero, generate code for fused multiply-add (0,1)
|
||||
# FETCH : how to fetch data in the inner loop:
|
||||
# 0 - load in a batch (the `normal way')
|
||||
# 1 - delay inner loop loads until just before they're needed
|
||||
|
||||
##############################################################################
|
||||
|
||||
require ("OptimizeUtil");
|
||||
|
||||
##############################################################################
|
||||
# optimize solver
|
||||
|
||||
sub testSolver # (filename)
|
||||
{
|
||||
my $filename = $_[0];
|
||||
createParametersFile ('ParametersT');
|
||||
$params = "$N1 $UNROLL1 $UNROLL2 $MADD $FETCH";
|
||||
print "***** TESTING $params\n";
|
||||
doit ("rm -f fastltsolve.c fastltsolve.o test_ldlt");
|
||||
doit ("make test_ldlt");
|
||||
doit ("./test_ldlt t >> $filename");
|
||||
open (FILE,">>$filename");
|
||||
print FILE " $params\n";
|
||||
close FILE;
|
||||
}
|
||||
|
||||
# find optimal parameters. UNROLL2 has no effect. write results to data5.txt
|
||||
|
||||
open (FILE,">data5.txt");
|
||||
print FILE "# solver data from OptimizeLDLT\n";
|
||||
close FILE;
|
||||
$FNAME='fastltsolve';
|
||||
$TYPE='t';
|
||||
$UNROLL2=1;
|
||||
for ($N1=1; $N1 <= 5; $N1++) {
|
||||
for ($UNROLL1=1; $UNROLL1 <= 15; $UNROLL1++) {
|
||||
for ($MADD=0; $MADD<=1; $MADD++) {
|
||||
for ($FETCH=0; $FETCH<=1; $FETCH++) {
|
||||
testSolver ('data5.txt');
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
readBackDataFile ('data5.txt');
|
||||
createParametersFile ('ParametersT');
|
||||
73
extern/ode/dist/ode/fbuild/OptimizeMultidot
vendored
73
extern/ode/dist/ode/fbuild/OptimizeMultidot
vendored
@@ -1,73 +0,0 @@
|
||||
#!/usr/bin/perl
|
||||
#
|
||||
#########################################################################
|
||||
# #
|
||||
# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. #
|
||||
# All rights reserved. Email: russ@q12.org Web: www.q12.org #
|
||||
# #
|
||||
# This library is free software; you can redistribute it and/or #
|
||||
# modify it under the terms of EITHER: #
|
||||
# (1) The GNU Lesser General Public License as published by the Free #
|
||||
# Software Foundation; either version 2.1 of the License, or (at #
|
||||
# your option) any later version. The text of the GNU Lesser #
|
||||
# General Public License is included with this library in the #
|
||||
# file LICENSE.TXT. #
|
||||
# (2) The BSD-style license that is included with this library in #
|
||||
# the file LICENSE-BSD.TXT. #
|
||||
# #
|
||||
# This library is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files #
|
||||
# LICENSE.TXT and LICENSE-BSD.TXT for more details. #
|
||||
# #
|
||||
#########################################################################
|
||||
|
||||
# optimize the dot product built by BuildMultidot
|
||||
|
||||
##############################################################################
|
||||
|
||||
require ("OptimizeUtil");
|
||||
|
||||
# multiple
|
||||
$N1=2;
|
||||
|
||||
# unused standard parameters
|
||||
$TYPE='unused';
|
||||
$UNROLL2=0; # unused
|
||||
$MADD=0; # unused
|
||||
|
||||
##############################################################################
|
||||
|
||||
sub testMultidot # (filename)
|
||||
{
|
||||
my $filename = $_[0];
|
||||
createParametersFile ('ParametersM');
|
||||
$params = "$N1 $UNROLL1 $UNROLL2 $MADD $FETCH $LAT1 $LAT2";
|
||||
print "***** TESTING $params\n";
|
||||
doit ("rm -f fastmultidot.c fastmultidot.o test_multidot");
|
||||
doit ("make test_multidot");
|
||||
doit ("./test_multidot >> $filename");
|
||||
open (FILE,">>$filename");
|
||||
print FILE " $params\n";
|
||||
close FILE;
|
||||
}
|
||||
|
||||
# find optimal parameters. write results to data6.txt
|
||||
|
||||
open (FILE,">data6.txt");
|
||||
print FILE "# multi-dot product data from OptimizeMultidot\n";
|
||||
close FILE;
|
||||
$FNAME='fastmultidot';
|
||||
|
||||
for ($UNROLL1=1; $UNROLL1 <= 10; $UNROLL1++) {
|
||||
for ($LAT1=1; $LAT1 <= 5; $LAT1++) {
|
||||
for ($LAT2=1; $LAT2 <= 5; $LAT2++) {
|
||||
for ($FETCH=1; $FETCH<=5; $FETCH++) {
|
||||
testMultidot ('data6.txt');
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
readBackDataFile ('data6.txt');
|
||||
createParametersFile ('ParametersM');
|
||||
86
extern/ode/dist/ode/fbuild/OptimizeUtil
vendored
86
extern/ode/dist/ode/fbuild/OptimizeUtil
vendored
@@ -1,86 +0,0 @@
|
||||
#!/usr/bin/perl -w
|
||||
#
|
||||
#########################################################################
|
||||
# #
|
||||
# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. #
|
||||
# All rights reserved. Email: russ@q12.org Web: www.q12.org #
|
||||
# #
|
||||
# This library is free software; you can redistribute it and/or #
|
||||
# modify it under the terms of EITHER: #
|
||||
# (1) The GNU Lesser General Public License as published by the Free #
|
||||
# Software Foundation; either version 2.1 of the License, or (at #
|
||||
# your option) any later version. The text of the GNU Lesser #
|
||||
# General Public License is included with this library in the #
|
||||
# file LICENSE.TXT. #
|
||||
# (2) The BSD-style license that is included with this library in #
|
||||
# the file LICENSE-BSD.TXT. #
|
||||
# #
|
||||
# This library is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files #
|
||||
# LICENSE.TXT and LICENSE-BSD.TXT for more details. #
|
||||
# #
|
||||
#########################################################################
|
||||
|
||||
package BuildUtil;
|
||||
|
||||
|
||||
sub main::doit
|
||||
{
|
||||
my $cmd = $_[0];
|
||||
print "$cmd\n";
|
||||
system ($cmd)==0 or die "FAILED";
|
||||
}
|
||||
|
||||
|
||||
sub main::createParametersFile # (filename)
|
||||
{
|
||||
open (PARAM,">$_[0]");
|
||||
print PARAM "# perl script to set parameters required by the code generator\n";
|
||||
print PARAM "\$FNAME=\"$main::FNAME\";\n" if defined($main::FNAME);
|
||||
print PARAM "\$TYPE=\"$main::TYPE\";\n" if defined($main::TYPE);
|
||||
print PARAM "\$N1=$main::N1;\n" if defined($main::N1);
|
||||
print PARAM "\$UNROLL1=$main::UNROLL1;\n" if defined($main::UNROLL1);
|
||||
print PARAM "\$UNROLL2=$main::UNROLL2;\n" if defined($main::UNROLL2);
|
||||
print PARAM "\$MADD=$main::MADD;\n" if defined($main::MADD);
|
||||
print PARAM "\$FETCH=$main::FETCH;\n" if defined($main::FETCH);
|
||||
print PARAM "\$LAT1=$main::LAT1;\n" if defined($main::LAT1);
|
||||
print PARAM "\$LAT2=$main::LAT2;\n" if defined($main::LAT2);
|
||||
close PARAM;
|
||||
}
|
||||
|
||||
|
||||
# read back a data file and find best parameters
|
||||
|
||||
sub main::readBackDataFile # (filename)
|
||||
{
|
||||
my $filename = $_[0];
|
||||
my $maxtime = 1e10;
|
||||
open (FILE,$filename);
|
||||
while (<FILE>) {
|
||||
next if /^\#/;
|
||||
my $line = lc $_;
|
||||
if ($line =~ /error/) {
|
||||
print "ERRORS FOUND IN $filename\n";
|
||||
exit 1;
|
||||
}
|
||||
$line =~ s/^\s*//;
|
||||
$line =~ s/\s*$//;
|
||||
my @nums = split (/\s+/,$line);
|
||||
$time = $nums[0];
|
||||
if ($time < $maxtime) {
|
||||
$main::N1 = $nums[1];
|
||||
$main::UNROLL1 = $nums[2];
|
||||
$main::UNROLL2 = $nums[3];
|
||||
$main::MADD = $nums[4];
|
||||
$main::FETCH = $nums[5];
|
||||
$main::LAT1 = $nums[6];
|
||||
$main::LAT2 = $nums[7];
|
||||
$maxtime = $time;
|
||||
}
|
||||
}
|
||||
close FILE;
|
||||
}
|
||||
|
||||
|
||||
1;
|
||||
32
extern/ode/dist/ode/fbuild/ParametersD.example
vendored
32
extern/ode/dist/ode/fbuild/ParametersD.example
vendored
@@ -1,32 +0,0 @@
|
||||
#########################################################################
|
||||
# #
|
||||
# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. #
|
||||
# All rights reserved. Email: russ@q12.org Web: www.q12.org #
|
||||
# #
|
||||
# This library is free software; you can redistribute it and/or #
|
||||
# modify it under the terms of EITHER: #
|
||||
# (1) The GNU Lesser General Public License as published by the Free #
|
||||
# Software Foundation; either version 2.1 of the License, or (at #
|
||||
# your option) any later version. The text of the GNU Lesser #
|
||||
# General Public License is included with this library in the #
|
||||
# file LICENSE.TXT. #
|
||||
# (2) The BSD-style license that is included with this library in #
|
||||
# the file LICENSE-BSD.TXT. #
|
||||
# #
|
||||
# This library is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files #
|
||||
# LICENSE.TXT and LICENSE-BSD.TXT for more details. #
|
||||
# #
|
||||
#########################################################################
|
||||
|
||||
# perl script to set parameters required by the code generator
|
||||
$FNAME="fastdot";
|
||||
$TYPE="unused";
|
||||
$N1=0;
|
||||
$UNROLL1=2;
|
||||
$UNROLL2=0;
|
||||
$MADD=0;
|
||||
$FETCH=1;
|
||||
$LAT1=1;
|
||||
$LAT2=2;
|
||||
30
extern/ode/dist/ode/fbuild/ParametersF.example
vendored
30
extern/ode/dist/ode/fbuild/ParametersF.example
vendored
@@ -1,30 +0,0 @@
|
||||
#########################################################################
|
||||
# #
|
||||
# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. #
|
||||
# All rights reserved. Email: russ@q12.org Web: www.q12.org #
|
||||
# #
|
||||
# This library is free software; you can redistribute it and/or #
|
||||
# modify it under the terms of EITHER: #
|
||||
# (1) The GNU Lesser General Public License as published by the Free #
|
||||
# Software Foundation; either version 2.1 of the License, or (at #
|
||||
# your option) any later version. The text of the GNU Lesser #
|
||||
# General Public License is included with this library in the #
|
||||
# file LICENSE.TXT. #
|
||||
# (2) The BSD-style license that is included with this library in #
|
||||
# the file LICENSE-BSD.TXT. #
|
||||
# #
|
||||
# This library is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files #
|
||||
# LICENSE.TXT and LICENSE-BSD.TXT for more details. #
|
||||
# #
|
||||
#########################################################################
|
||||
|
||||
# perl script to set parameters required by the code generator
|
||||
$FNAME="fastldlt";
|
||||
$TYPE="f";
|
||||
$N1=2;
|
||||
$UNROLL1=2;
|
||||
$UNROLL2=6;
|
||||
$MADD=0;
|
||||
$FETCH=1;
|
||||
32
extern/ode/dist/ode/fbuild/ParametersM.example
vendored
32
extern/ode/dist/ode/fbuild/ParametersM.example
vendored
@@ -1,32 +0,0 @@
|
||||
#########################################################################
|
||||
# #
|
||||
# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. #
|
||||
# All rights reserved. Email: russ@q12.org Web: www.q12.org #
|
||||
# #
|
||||
# This library is free software; you can redistribute it and/or #
|
||||
# modify it under the terms of EITHER: #
|
||||
# (1) The GNU Lesser General Public License as published by the Free #
|
||||
# Software Foundation; either version 2.1 of the License, or (at #
|
||||
# your option) any later version. The text of the GNU Lesser #
|
||||
# General Public License is included with this library in the #
|
||||
# file LICENSE.TXT. #
|
||||
# (2) The BSD-style license that is included with this library in #
|
||||
# the file LICENSE-BSD.TXT. #
|
||||
# #
|
||||
# This library is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files #
|
||||
# LICENSE.TXT and LICENSE-BSD.TXT for more details. #
|
||||
# #
|
||||
#########################################################################
|
||||
|
||||
# perl script to set parameters required by the code generator
|
||||
$FNAME="fastmultidot";
|
||||
$TYPE="unused";
|
||||
$N1=2;
|
||||
$UNROLL1=1;
|
||||
$UNROLL2=0;
|
||||
$MADD=0;
|
||||
$FETCH=5;
|
||||
$LAT1=1;
|
||||
$LAT2=1;
|
||||
30
extern/ode/dist/ode/fbuild/ParametersS.example
vendored
30
extern/ode/dist/ode/fbuild/ParametersS.example
vendored
@@ -1,30 +0,0 @@
|
||||
#########################################################################
|
||||
# #
|
||||
# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. #
|
||||
# All rights reserved. Email: russ@q12.org Web: www.q12.org #
|
||||
# #
|
||||
# This library is free software; you can redistribute it and/or #
|
||||
# modify it under the terms of EITHER: #
|
||||
# (1) The GNU Lesser General Public License as published by the Free #
|
||||
# Software Foundation; either version 2.1 of the License, or (at #
|
||||
# your option) any later version. The text of the GNU Lesser #
|
||||
# General Public License is included with this library in the #
|
||||
# file LICENSE.TXT. #
|
||||
# (2) The BSD-style license that is included with this library in #
|
||||
# the file LICENSE-BSD.TXT. #
|
||||
# #
|
||||
# This library is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files #
|
||||
# LICENSE.TXT and LICENSE-BSD.TXT for more details. #
|
||||
# #
|
||||
#########################################################################
|
||||
|
||||
# perl script to set parameters required by the code generator
|
||||
$FNAME="fastlsolve";
|
||||
$TYPE="s";
|
||||
$N1=4;
|
||||
$UNROLL1=12;
|
||||
$UNROLL2=1;
|
||||
$MADD=1;
|
||||
$FETCH=0;
|
||||
30
extern/ode/dist/ode/fbuild/ParametersT.example
vendored
30
extern/ode/dist/ode/fbuild/ParametersT.example
vendored
@@ -1,30 +0,0 @@
|
||||
#########################################################################
|
||||
# #
|
||||
# Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. #
|
||||
# All rights reserved. Email: russ@q12.org Web: www.q12.org #
|
||||
# #
|
||||
# This library is free software; you can redistribute it and/or #
|
||||
# modify it under the terms of EITHER: #
|
||||
# (1) The GNU Lesser General Public License as published by the Free #
|
||||
# Software Foundation; either version 2.1 of the License, or (at #
|
||||
# your option) any later version. The text of the GNU Lesser #
|
||||
# General Public License is included with this library in the #
|
||||
# file LICENSE.TXT. #
|
||||
# (2) The BSD-style license that is included with this library in #
|
||||
# the file LICENSE-BSD.TXT. #
|
||||
# #
|
||||
# This library is distributed in the hope that it will be useful, #
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of #
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files #
|
||||
# LICENSE.TXT and LICENSE-BSD.TXT for more details. #
|
||||
# #
|
||||
#########################################################################
|
||||
|
||||
# perl script to set parameters required by the code generator
|
||||
$FNAME="fastltsolve";
|
||||
$TYPE="t";
|
||||
$N1=4;
|
||||
$UNROLL1=12;
|
||||
$UNROLL2=1;
|
||||
$MADD=1;
|
||||
$FETCH=0;
|
||||
41
extern/ode/dist/ode/fbuild/README
vendored
41
extern/ode/dist/ode/fbuild/README
vendored
@@ -1,41 +0,0 @@
|
||||
|
||||
factorizer/solver builder
|
||||
|
||||
before running `make', copy the following files:
|
||||
ParametersD.example --> ParametersD
|
||||
ParametersF.example --> ParametersF
|
||||
ParametersS.example --> ParametersS
|
||||
|
||||
the files Parameters[D|F|S] don't exist in the CVS archive because
|
||||
they are changable.
|
||||
|
||||
|
||||
|
||||
STATS - for chol
|
||||
-----
|
||||
|
||||
* all with -O1
|
||||
|
||||
128x128 matrix
|
||||
atlas = 1724779 clocks
|
||||
my chol = 1164629 clocks (parameters: 2 2 2 1) with Ai++, Aj++
|
||||
my chol = 1140786 clocks (parameters: 2 6 8 0) with Ai++, Aj++
|
||||
my chol = 1118968 clocks (parameters: 2 6 8 0) with +ofs
|
||||
|
||||
64x64 matrix
|
||||
atlas = 374020 clocks
|
||||
my chol = 157076 clocks (parameters = 2 2 2 1)
|
||||
|
||||
32x32 matrix (12961 flops)
|
||||
atlas = 83827 clocks
|
||||
my chol = 25945 clocks (parameters: 2 2 2 1)
|
||||
|
||||
|
||||
|
||||
|
||||
TODO
|
||||
----
|
||||
|
||||
* doc!
|
||||
|
||||
* iterate blocks by partial rows to try and keep more data in cache
|
||||
26
extern/ode/dist/ode/fbuild/ldlt.m
vendored
26
extern/ode/dist/ode/fbuild/ldlt.m
vendored
@@ -1,26 +0,0 @@
|
||||
function [L,d] = ldlt(A)
|
||||
|
||||
n=length(A);
|
||||
d=zeros(n,1);
|
||||
|
||||
d(1) = 1/A(1,1);
|
||||
for i=2:n
|
||||
for j=2:i-1
|
||||
A(i,j) = A(i,j) - A(j,1:j-1) * A(i,1:j-1)';
|
||||
end
|
||||
sum = 0;
|
||||
for j=1:i-1
|
||||
q1 = A(i,j);
|
||||
q2 = q1 * d(j);
|
||||
A(i,j) = q2;
|
||||
sum = sum + q1*q2;
|
||||
end
|
||||
d(i) = 1/(A(i,i) - sum);
|
||||
end
|
||||
|
||||
L=A;
|
||||
for i=1:n
|
||||
L(i,i:n)=zeros(1,n+1-i);
|
||||
L(i,i)=1;
|
||||
end
|
||||
d = d .\ 1;
|
||||
124
extern/ode/dist/ode/fbuild/test_dot.cpp
vendored
124
extern/ode/dist/ode/fbuild/test_dot.cpp
vendored
@@ -1,124 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#include <stdio.h>
|
||||
#include "ode/ode.h"
|
||||
|
||||
#define ALLOCA dALLOCA16
|
||||
#define SIZE 1000
|
||||
|
||||
|
||||
// correct dot product, for accuracy testing
|
||||
|
||||
dReal goodDot (dReal *a, dReal *b, int n)
|
||||
{
|
||||
dReal sum=0;
|
||||
while (n > 0) {
|
||||
sum += (*a) * (*b);
|
||||
a++;
|
||||
b++;
|
||||
n--;
|
||||
}
|
||||
return sum;
|
||||
}
|
||||
|
||||
|
||||
// test dot product accuracy
|
||||
|
||||
void testAccuracy()
|
||||
{
|
||||
// allocate vectors a and b and fill them with random data
|
||||
dReal *a = (dReal*) ALLOCA (SIZE*sizeof(dReal));
|
||||
dReal *b = (dReal*) ALLOCA (SIZE*sizeof(dReal));
|
||||
dMakeRandomMatrix (a,1,SIZE,1.0);
|
||||
dMakeRandomMatrix (b,1,SIZE,1.0);
|
||||
|
||||
for (int n=1; n<100; n++) {
|
||||
dReal good = goodDot (a,b,n);
|
||||
dReal test = dDot (a,b,n);
|
||||
dReal diff = fabs(good-test);
|
||||
//printf ("diff = %e\n",diff);
|
||||
if (diff > 1e-10) printf ("ERROR: accuracy test failed\n");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// test dot product factorizer speed.
|
||||
|
||||
void testSpeed()
|
||||
{
|
||||
// allocate vectors a and b and fill them with random data
|
||||
dReal *a = (dReal*) ALLOCA (SIZE*sizeof(dReal));
|
||||
dReal *b = (dReal*) ALLOCA (SIZE*sizeof(dReal));
|
||||
dMakeRandomMatrix (a,1,SIZE,1.0);
|
||||
dMakeRandomMatrix (b,1,SIZE,1.0);
|
||||
|
||||
// time several dot products, return the minimum timing
|
||||
double mintime = 1e100;
|
||||
dStopwatch sw;
|
||||
for (int i=0; i<1000; i++) {
|
||||
dStopwatchReset (&sw);
|
||||
dStopwatchStart (&sw);
|
||||
|
||||
// try a bunch of prime sizes up to 101
|
||||
dDot (a,b,2);
|
||||
dDot (a,b,3);
|
||||
dDot (a,b,5);
|
||||
dDot (a,b,7);
|
||||
dDot (a,b,11);
|
||||
dDot (a,b,13);
|
||||
dDot (a,b,17);
|
||||
dDot (a,b,19);
|
||||
dDot (a,b,23);
|
||||
dDot (a,b,29);
|
||||
dDot (a,b,31);
|
||||
dDot (a,b,37);
|
||||
dDot (a,b,41);
|
||||
dDot (a,b,43);
|
||||
dDot (a,b,47);
|
||||
dDot (a,b,53);
|
||||
dDot (a,b,59);
|
||||
dDot (a,b,61);
|
||||
dDot (a,b,67);
|
||||
dDot (a,b,71);
|
||||
dDot (a,b,73);
|
||||
dDot (a,b,79);
|
||||
dDot (a,b,83);
|
||||
dDot (a,b,89);
|
||||
dDot (a,b,97);
|
||||
dDot (a,b,101);
|
||||
|
||||
dStopwatchStop (&sw);
|
||||
double time = dStopwatchTime (&sw);
|
||||
if (time < mintime) mintime = time;
|
||||
}
|
||||
|
||||
printf ("%.0f",mintime * dTimerTicksPerSecond());
|
||||
}
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
testAccuracy();
|
||||
testSpeed();
|
||||
return 0;
|
||||
}
|
||||
299
extern/ode/dist/ode/fbuild/test_ldlt.cpp
vendored
299
extern/ode/dist/ode/fbuild/test_ldlt.cpp
vendored
@@ -1,299 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <malloc.h>
|
||||
#include "ode/ode.h"
|
||||
|
||||
#define ALLOCA dALLOCA16
|
||||
|
||||
//****************************************************************************
|
||||
// constants
|
||||
|
||||
#ifdef dSINGLE
|
||||
#define TOL (1e-4)
|
||||
#else
|
||||
#define TOL (1e-10)
|
||||
#endif
|
||||
|
||||
//****************************************************************************
|
||||
// test L*X=B solver accuracy.
|
||||
|
||||
void testSolverAccuracy (int n)
|
||||
{
|
||||
int i;
|
||||
int npad = dPAD(n);
|
||||
dReal *L = (dReal*) ALLOCA (n*npad*sizeof(dReal));
|
||||
dReal *B = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *B2 = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *X = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
|
||||
// L is a random lower triangular matrix with 1's on the diagonal
|
||||
dMakeRandomMatrix (L,n,n,1.0);
|
||||
dClearUpperTriangle (L,n);
|
||||
for (i=0; i<n; i++) L[i*npad+i] = 1;
|
||||
|
||||
// B is the right hand side
|
||||
dMakeRandomMatrix (B,n,1,1.0);
|
||||
memcpy (X,B,n*sizeof(dReal)); // copy B to X
|
||||
|
||||
dSolveL1 (L,X,n,npad);
|
||||
|
||||
/*
|
||||
dPrintMatrix (L,n,n);
|
||||
printf ("\n");
|
||||
dPrintMatrix (B,n,1);
|
||||
printf ("\n");
|
||||
dPrintMatrix (X,n,1);
|
||||
printf ("\n");
|
||||
*/
|
||||
|
||||
dSetZero (B2,n);
|
||||
dMultiply0 (B2,L,X,n,n,1);
|
||||
dReal error = dMaxDifference (B,B2,1,n);
|
||||
if (error > TOL) {
|
||||
printf ("error = %e, size = %d\n",error,n);
|
||||
}
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// test L^T*X=B solver accuracy.
|
||||
|
||||
void testTransposeSolverAccuracy (int n)
|
||||
{
|
||||
int i;
|
||||
int npad = dPAD(n);
|
||||
dReal *L = (dReal*) ALLOCA (n*npad*sizeof(dReal));
|
||||
dReal *B = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *B2 = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *X = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
|
||||
// L is a random lower triangular matrix with 1's on the diagonal
|
||||
dMakeRandomMatrix (L,n,n,1.0);
|
||||
dClearUpperTriangle (L,n);
|
||||
for (i=0; i<n; i++) L[i*npad+i] = 1;
|
||||
|
||||
// B is the right hand side
|
||||
dMakeRandomMatrix (B,n,1,1.0);
|
||||
memcpy (X,B,n*sizeof(dReal)); // copy B to X
|
||||
|
||||
dSolveL1T (L,X,n,npad);
|
||||
|
||||
dSetZero (B2,n);
|
||||
dMultiply1 (B2,L,X,n,n,1);
|
||||
dReal error = dMaxDifference (B,B2,1,n);
|
||||
if (error > TOL) {
|
||||
printf ("error = %e, size = %d\n",error,n);
|
||||
}
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// test L*D*L' factorizer accuracy.
|
||||
|
||||
void testLDLTAccuracy (int n)
|
||||
{
|
||||
int i,j;
|
||||
int npad = dPAD(n);
|
||||
dReal *A = (dReal*) ALLOCA (n*npad*sizeof(dReal));
|
||||
dReal *L = (dReal*) ALLOCA (n*npad*sizeof(dReal));
|
||||
dReal *d = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *Atest = (dReal*) ALLOCA (n*npad*sizeof(dReal));
|
||||
dReal *DL = (dReal*) ALLOCA (n*npad*sizeof(dReal));
|
||||
|
||||
dMakeRandomMatrix (A,n,n,1.0);
|
||||
dMultiply2 (L,A,A,n,n,n);
|
||||
memcpy (A,L,n*npad*sizeof(dReal));
|
||||
dSetZero (d,n);
|
||||
|
||||
dFactorLDLT (L,d,n,npad);
|
||||
|
||||
// make L lower triangular, and convert d into diagonal of D
|
||||
dClearUpperTriangle (L,n);
|
||||
for (i=0; i<n; i++) L[i*npad+i] = 1;
|
||||
for (i=0; i<n; i++) d[i] = 1.0/d[i];
|
||||
|
||||
// form Atest = L*D*L'
|
||||
dSetZero (Atest,n*npad);
|
||||
dSetZero (DL,n*npad);
|
||||
for (i=0; i<n; i++) {
|
||||
for (j=0; j<n; j++) DL[i*npad+j] = L[i*npad+j] * d[j];
|
||||
}
|
||||
dMultiply2 (Atest,L,DL,n,n,n);
|
||||
dReal error = dMaxDifference (A,Atest,n,n);
|
||||
if (error > TOL) {
|
||||
printf ("error = %e, size = %d\n",error,n);
|
||||
}
|
||||
|
||||
/*
|
||||
printf ("\n");
|
||||
dPrintMatrix (A,n,n);
|
||||
printf ("\n");
|
||||
dPrintMatrix (L,n,n);
|
||||
printf ("\n");
|
||||
dPrintMatrix (d,1,n);
|
||||
*/
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// test L*D*L' factorizer speed.
|
||||
|
||||
void testLDLTSpeed (int n)
|
||||
{
|
||||
int npad = dPAD(n);
|
||||
|
||||
// allocate A
|
||||
dReal *A = (dReal*) ALLOCA (n*npad*sizeof(dReal));
|
||||
|
||||
// make B a symmetric positive definite matrix
|
||||
dMakeRandomMatrix (A,n,n,1.0);
|
||||
dReal *B = (dReal*) ALLOCA (n*npad*sizeof(dReal));
|
||||
dSetZero (B,n*npad);
|
||||
dMultiply2 (B,A,A,n,n,n);
|
||||
|
||||
// make d
|
||||
dReal *d = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dSetZero (d,n);
|
||||
|
||||
// time several factorizations, return the minimum timing
|
||||
double mintime = 1e100;
|
||||
dStopwatch sw;
|
||||
for (int i=0; i<100; i++) {
|
||||
memcpy (A,B,n*npad*sizeof(dReal));
|
||||
dStopwatchReset (&sw);
|
||||
dStopwatchStart (&sw);
|
||||
|
||||
dFactorLDLT (A,d,n,npad);
|
||||
|
||||
dStopwatchStop (&sw);
|
||||
double time = dStopwatchTime (&sw);
|
||||
if (time < mintime) mintime = time;
|
||||
}
|
||||
|
||||
printf ("%.0f",mintime * dTimerTicksPerSecond());
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// test solver speed.
|
||||
|
||||
void testSolverSpeed (int n, int transpose)
|
||||
{
|
||||
int i;
|
||||
int npad = dPAD(n);
|
||||
|
||||
// allocate L,B,X
|
||||
dReal *L = (dReal*) ALLOCA (n*npad*sizeof(dReal));
|
||||
dReal *B = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *X = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
|
||||
// L is a random lower triangular matrix with 1's on the diagonal
|
||||
dMakeRandomMatrix (L,n,n,1.0);
|
||||
dClearUpperTriangle (L,n);
|
||||
for (i=0; i<n; i++) L[i*npad+i] = 1;
|
||||
|
||||
// B is the right hand side
|
||||
dMakeRandomMatrix (B,n,1,1.0);
|
||||
|
||||
// time several factorizations, return the minimum timing
|
||||
double mintime = 1e100;
|
||||
dStopwatch sw;
|
||||
for (int i=0; i<100; i++) {
|
||||
memcpy (X,B,n*sizeof(dReal)); // copy B to X
|
||||
|
||||
if (transpose) {
|
||||
dStopwatchReset (&sw);
|
||||
dStopwatchStart (&sw);
|
||||
dSolveL1T (L,X,n,npad);
|
||||
dStopwatchStop (&sw);
|
||||
}
|
||||
else {
|
||||
dStopwatchReset (&sw);
|
||||
dStopwatchStart (&sw);
|
||||
dSolveL1 (L,X,n,npad);
|
||||
dStopwatchStop (&sw);
|
||||
}
|
||||
|
||||
double time = dStopwatchTime (&sw);
|
||||
if (time < mintime) mintime = time;
|
||||
}
|
||||
|
||||
printf ("%.0f",mintime * dTimerTicksPerSecond());
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// the single command line argument is 'f' to test and time the factorizer,
|
||||
// or 's' to test and time the solver.
|
||||
|
||||
|
||||
void testAccuracy (int n, char type)
|
||||
{
|
||||
if (type == 'f') testLDLTAccuracy (n);
|
||||
if (type == 's') testSolverAccuracy (n);
|
||||
if (type == 't') testTransposeSolverAccuracy (n);
|
||||
}
|
||||
|
||||
|
||||
void testSpeed (int n, char type)
|
||||
{
|
||||
if (type == 'f') testLDLTSpeed (n);
|
||||
if (type == 's') testSolverSpeed (n,0);
|
||||
if (type == 't') testSolverSpeed (n,1);
|
||||
}
|
||||
|
||||
|
||||
int main (int argc, char **argv)
|
||||
{
|
||||
if (argc != 2 || argv[1][0] == 0 || argv[1][1] != 0 ||
|
||||
(argv[1][0] != 'f' && argv[1][0] != 's' && argv[1][0] != 't')) {
|
||||
fprintf (stderr,"Usage: test_ldlt [f|s|t]\n");
|
||||
exit (1);
|
||||
}
|
||||
char type = argv[1][0];
|
||||
|
||||
// accuracy test: test all sizes up to 20 then all prime sizes up to 101
|
||||
int i;
|
||||
for (i=1; i<20; i++) {
|
||||
testAccuracy (i,type);
|
||||
}
|
||||
testAccuracy (23,type);
|
||||
testAccuracy (29,type);
|
||||
testAccuracy (31,type);
|
||||
testAccuracy (37,type);
|
||||
testAccuracy (41,type);
|
||||
testAccuracy (43,type);
|
||||
testAccuracy (47,type);
|
||||
testAccuracy (53,type);
|
||||
testAccuracy (59,type);
|
||||
testAccuracy (61,type);
|
||||
testAccuracy (67,type);
|
||||
testAccuracy (71,type);
|
||||
testAccuracy (73,type);
|
||||
testAccuracy (79,type);
|
||||
testAccuracy (83,type);
|
||||
testAccuracy (89,type);
|
||||
testAccuracy (97,type);
|
||||
testAccuracy (101,type);
|
||||
|
||||
// test speed on a 127x127 matrix
|
||||
testSpeed (127,type);
|
||||
|
||||
return 0;
|
||||
}
|
||||
144
extern/ode/dist/ode/fbuild/test_multidot.cpp
vendored
144
extern/ode/dist/ode/fbuild/test_multidot.cpp
vendored
@@ -1,144 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#include <stdio.h>
|
||||
#include "ode/ode.h"
|
||||
|
||||
#define NUM_A 2
|
||||
#define ALLOCA dALLOCA16
|
||||
#define SIZE 1000
|
||||
|
||||
|
||||
extern "C" void dMultidot2 (const dReal *a0, const dReal *a1,
|
||||
const dReal *b, dReal *outsum, int n);
|
||||
/*
|
||||
extern "C" void dMultidot4 (const dReal *a0, const dReal *a1,
|
||||
const dReal *a2, const dReal *a3,
|
||||
const dReal *b, dReal *outsum, int n);
|
||||
*/
|
||||
|
||||
|
||||
// correct dot product, for accuracy testing
|
||||
|
||||
dReal goodDot (dReal *a, dReal *b, int n)
|
||||
{
|
||||
dReal sum=0;
|
||||
while (n > 0) {
|
||||
sum += (*a) * (*b);
|
||||
a++;
|
||||
b++;
|
||||
n--;
|
||||
}
|
||||
return sum;
|
||||
}
|
||||
|
||||
|
||||
// test multi-dot product accuracy
|
||||
|
||||
void testAccuracy()
|
||||
{
|
||||
int j;
|
||||
|
||||
// allocate vectors a and b and fill them with random data
|
||||
dReal *a[NUM_A];
|
||||
for (j=0; j<NUM_A; j++) a[j] = (dReal*) ALLOCA (SIZE*sizeof(dReal));
|
||||
dReal *b = (dReal*) ALLOCA (SIZE*sizeof(dReal));
|
||||
for (j=0; j<NUM_A; j++) dMakeRandomMatrix (a[j],1,SIZE,1.0);
|
||||
dMakeRandomMatrix (b,1,SIZE,1.0);
|
||||
|
||||
for (int n=1; n<100; n++) {
|
||||
dReal good[NUM_A];
|
||||
for (j=0; j<NUM_A; j++) good[j] = goodDot (a[j],b,n);
|
||||
dReal test[4];
|
||||
dMultidot2 (a[0],a[1],b,test,n);
|
||||
dReal diff = 0;
|
||||
for (j=0; j<NUM_A; j++) diff += fabs(good[j]-test[j]);
|
||||
// printf ("diff = %e\n",diff);
|
||||
if (diff > 1e-10) printf ("ERROR: accuracy test failed\n");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// test multi-dot product factorizer speed.
|
||||
|
||||
void testSpeed()
|
||||
{
|
||||
int j;
|
||||
dReal sum[NUM_A];
|
||||
|
||||
// allocate vectors a and b and fill them with random data
|
||||
dReal *a[NUM_A];
|
||||
for (j=0; j<NUM_A; j++) a[j] = (dReal*) ALLOCA (SIZE*sizeof(dReal));
|
||||
dReal *b = (dReal*) ALLOCA (SIZE*sizeof(dReal));
|
||||
for (j=0; j<NUM_A; j++) dMakeRandomMatrix (a[j],1,SIZE,1.0);
|
||||
dMakeRandomMatrix (b,1,SIZE,1.0);
|
||||
|
||||
// time several dot products, return the minimum timing
|
||||
double mintime = 1e100;
|
||||
dStopwatch sw;
|
||||
for (int i=0; i<1000; i++) {
|
||||
dStopwatchReset (&sw);
|
||||
dStopwatchStart (&sw);
|
||||
|
||||
// try a bunch of prime sizes up to 101
|
||||
dMultidot2 (a[0],a[1],b,sum,2);
|
||||
dMultidot2 (a[0],a[1],b,sum,3);
|
||||
dMultidot2 (a[0],a[1],b,sum,5);
|
||||
dMultidot2 (a[0],a[1],b,sum,7);
|
||||
dMultidot2 (a[0],a[1],b,sum,11);
|
||||
dMultidot2 (a[0],a[1],b,sum,13);
|
||||
dMultidot2 (a[0],a[1],b,sum,17);
|
||||
dMultidot2 (a[0],a[1],b,sum,19);
|
||||
dMultidot2 (a[0],a[1],b,sum,23);
|
||||
dMultidot2 (a[0],a[1],b,sum,29);
|
||||
dMultidot2 (a[0],a[1],b,sum,31);
|
||||
dMultidot2 (a[0],a[1],b,sum,37);
|
||||
dMultidot2 (a[0],a[1],b,sum,41);
|
||||
dMultidot2 (a[0],a[1],b,sum,43);
|
||||
dMultidot2 (a[0],a[1],b,sum,47);
|
||||
dMultidot2 (a[0],a[1],b,sum,53);
|
||||
dMultidot2 (a[0],a[1],b,sum,59);
|
||||
dMultidot2 (a[0],a[1],b,sum,61);
|
||||
dMultidot2 (a[0],a[1],b,sum,67);
|
||||
dMultidot2 (a[0],a[1],b,sum,71);
|
||||
dMultidot2 (a[0],a[1],b,sum,73);
|
||||
dMultidot2 (a[0],a[1],b,sum,79);
|
||||
dMultidot2 (a[0],a[1],b,sum,83);
|
||||
dMultidot2 (a[0],a[1],b,sum,89);
|
||||
dMultidot2 (a[0],a[1],b,sum,97);
|
||||
dMultidot2 (a[0],a[1],b,sum,101);
|
||||
|
||||
dStopwatchStop (&sw);
|
||||
double time = dStopwatchTime (&sw);
|
||||
if (time < mintime) mintime = time;
|
||||
}
|
||||
|
||||
printf ("%.0f",mintime * dTimerTicksPerSecond());
|
||||
}
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
testAccuracy();
|
||||
testSpeed();
|
||||
return 0;
|
||||
}
|
||||
80
extern/ode/dist/ode/src/array.cpp
vendored
80
extern/ode/dist/ode/src/array.cpp
vendored
@@ -1,80 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#include <ode/config.h>
|
||||
#include <ode/memory.h>
|
||||
#include <ode/error.h>
|
||||
#include "array.h"
|
||||
|
||||
|
||||
static inline int roundUpToPowerOfTwo (int x)
|
||||
{
|
||||
int i = 1;
|
||||
while (i < x) i <<= 1;
|
||||
return i;
|
||||
}
|
||||
|
||||
|
||||
void dArrayBase::_freeAll (int sizeofT)
|
||||
{
|
||||
if (_data) {
|
||||
if (_data == this+1) return; // if constructLocalArray() was called
|
||||
dFree (_data,_anum * sizeofT);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dArrayBase::_setSize (int newsize, int sizeofT)
|
||||
{
|
||||
if (newsize < 0) return;
|
||||
if (newsize > _anum) {
|
||||
if (_data == this+1) {
|
||||
// this is a no-no, because constructLocalArray() was called
|
||||
dDebug (0,"setSize() out of space in LOCAL array");
|
||||
}
|
||||
int newanum = roundUpToPowerOfTwo (newsize);
|
||||
if (_data) _data = dRealloc (_data, _anum*sizeofT, newanum*sizeofT);
|
||||
else _data = dAlloc (newanum*sizeofT);
|
||||
_anum = newanum;
|
||||
}
|
||||
_size = newsize;
|
||||
}
|
||||
|
||||
|
||||
void * dArrayBase::operator new (size_t size)
|
||||
{
|
||||
return dAlloc (size);
|
||||
}
|
||||
|
||||
|
||||
void dArrayBase::operator delete (void *ptr, size_t size)
|
||||
{
|
||||
dFree (ptr,size);
|
||||
}
|
||||
|
||||
|
||||
void dArrayBase::constructLocalArray (int __anum)
|
||||
{
|
||||
_size = 0;
|
||||
_anum = __anum;
|
||||
_data = this+1;
|
||||
}
|
||||
135
extern/ode/dist/ode/src/array.h
vendored
135
extern/ode/dist/ode/src/array.h
vendored
@@ -1,135 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
/* this comes from the `reuse' library. copy any changes back to the source.
|
||||
*
|
||||
* Variable sized array template. The array is always stored in a contiguous
|
||||
* chunk. The array can be resized. A size increase will cause more memory
|
||||
* to be allocated, and may result in relocation of the array memory.
|
||||
* A size decrease has no effect on the memory allocation.
|
||||
*
|
||||
* Array elements with constructors or destructors are not supported!
|
||||
* But if you must have such elements, here's what to know/do:
|
||||
* - Bitwise copy is used when copying whole arrays.
|
||||
* - When copying individual items (via push(), insert() etc) the `='
|
||||
* (equals) operator is used. Thus you should define this operator to do
|
||||
* a bitwise copy. You should probably also define the copy constructor.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _ODE_ARRAY_H_
|
||||
#define _ODE_ARRAY_H_
|
||||
|
||||
#include <ode/config.h>
|
||||
|
||||
|
||||
// this base class has no constructors or destructor, for your convenience.
|
||||
|
||||
class dArrayBase {
|
||||
protected:
|
||||
int _size; // number of elements in `data'
|
||||
int _anum; // allocated number of elements in `data'
|
||||
void *_data; // array data
|
||||
|
||||
void _freeAll (int sizeofT);
|
||||
void _setSize (int newsize, int sizeofT);
|
||||
// set the array size to `newsize', allocating more memory if necessary.
|
||||
// if newsize>_anum and is a power of two then this is guaranteed to
|
||||
// set _size and _anum to newsize.
|
||||
|
||||
public:
|
||||
// not: dArrayBase () { _size=0; _anum=0; _data=0; }
|
||||
|
||||
int size() const { return _size; }
|
||||
int allocatedSize() const { return _anum; }
|
||||
void * operator new (size_t size);
|
||||
void operator delete (void *ptr, size_t size);
|
||||
|
||||
void constructor() { _size=0; _anum=0; _data=0; }
|
||||
// if this structure is allocated with malloc() instead of new, you can
|
||||
// call this to set it up.
|
||||
|
||||
void constructLocalArray (int __anum);
|
||||
// this helper function allows non-reallocating arrays to be constructed
|
||||
// on the stack (or in the heap if necessary). this is something of a
|
||||
// kludge and should be used with extreme care. this function acts like
|
||||
// a constructor - it is called on uninitialized memory that will hold the
|
||||
// Array structure and the data. __anum is the number of elements that
|
||||
// are allocated. the memory MUST be allocated with size:
|
||||
// sizeof(ArrayBase) + __anum*sizeof(T)
|
||||
// arrays allocated this way will never try to reallocate or free the
|
||||
// memory - that's your job.
|
||||
};
|
||||
|
||||
|
||||
template <class T> class dArray : public dArrayBase {
|
||||
public:
|
||||
void equals (const dArray<T> &x) {
|
||||
setSize (x.size());
|
||||
memcpy (_data,x._data,x._size * sizeof(T));
|
||||
}
|
||||
|
||||
dArray () { constructor(); }
|
||||
dArray (const dArray<T> &x) { constructor(); equals (x); }
|
||||
~dArray () { _freeAll(sizeof(T)); }
|
||||
void setSize (int newsize) { _setSize (newsize,sizeof(T)); }
|
||||
T *data() const { return (T*) _data; }
|
||||
T & operator[] (int i) const { return ((T*)_data)[i]; }
|
||||
void operator = (const dArray<T> &x) { equals (x); }
|
||||
|
||||
void push (const T item) {
|
||||
if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T));
|
||||
((T*)_data)[_size-1] = item;
|
||||
}
|
||||
|
||||
void swap (dArray<T> &x) {
|
||||
int tmp1;
|
||||
void *tmp2;
|
||||
tmp1=_size; _size=x._size; x._size=tmp1;
|
||||
tmp1=_anum; _anum=x._anum; x._anum=tmp1;
|
||||
tmp2=_data; _data=x._data; x._data=tmp2;
|
||||
}
|
||||
|
||||
// insert the item at the position `i'. if i<0 then add the item to the
|
||||
// start, if i >= size then add the item to the end of the array.
|
||||
void insert (int i, const T item) {
|
||||
if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T));
|
||||
if (i >= (_size-1)) i = _size-1; // add to end
|
||||
else {
|
||||
if (i < 0) i=0; // add to start
|
||||
int n = _size-1-i;
|
||||
if (n>0) memmove (((T*)_data) + i+1, ((T*)_data) + i, n*sizeof(T));
|
||||
}
|
||||
((T*)_data)[i] = item;
|
||||
}
|
||||
|
||||
void remove (int i) {
|
||||
if (i >= 0 && i < _size) { // passing this test guarantees size>0
|
||||
int n = _size-1-i;
|
||||
if (n>0) memmove (((T*)_data) + i, ((T*)_data) + i+1, n*sizeof(T));
|
||||
_size--;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
172
extern/ode/dist/ode/src/error.cpp
vendored
172
extern/ode/dist/ode/src/error.cpp
vendored
@@ -1,172 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#include <ode/config.h>
|
||||
#include <ode/error.h>
|
||||
|
||||
|
||||
static dMessageFunction *error_function = 0;
|
||||
static dMessageFunction *debug_function = 0;
|
||||
static dMessageFunction *message_function = 0;
|
||||
|
||||
|
||||
extern "C" void dSetErrorHandler (dMessageFunction *fn)
|
||||
{
|
||||
error_function = fn;
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dSetDebugHandler (dMessageFunction *fn)
|
||||
{
|
||||
debug_function = fn;
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dSetMessageHandler (dMessageFunction *fn)
|
||||
{
|
||||
message_function = fn;
|
||||
}
|
||||
|
||||
|
||||
extern "C" dMessageFunction *dGetErrorHandler()
|
||||
{
|
||||
return error_function;
|
||||
}
|
||||
|
||||
|
||||
extern "C" dMessageFunction *dGetDebugHandler()
|
||||
{
|
||||
return debug_function;
|
||||
}
|
||||
|
||||
|
||||
extern "C" dMessageFunction *dGetMessageHandler()
|
||||
{
|
||||
return message_function;
|
||||
}
|
||||
|
||||
|
||||
static void printMessage (int num, const char *msg1, const char *msg2,
|
||||
va_list ap)
|
||||
{
|
||||
fflush (stderr);
|
||||
fflush (stdout);
|
||||
if (num) fprintf (stderr,"\n%s %d: ",msg1,num);
|
||||
else fprintf (stderr,"\n%s: ",msg1);
|
||||
vfprintf (stderr,msg2,ap);
|
||||
fprintf (stderr,"\n");
|
||||
fflush (stderr);
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// unix
|
||||
|
||||
#ifndef WIN32
|
||||
|
||||
extern "C" void dError (int num, const char *msg, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start (ap,msg);
|
||||
if (error_function) error_function (num,msg,ap);
|
||||
else printMessage (num,"ODE Error",msg,ap);
|
||||
exit (1);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dDebug (int num, const char *msg, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start (ap,msg);
|
||||
if (debug_function) debug_function (num,msg,ap);
|
||||
else printMessage (num,"ODE INTERNAL ERROR",msg,ap);
|
||||
// *((char *)0) = 0; ... commit SEGVicide
|
||||
abort();
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dMessage (int num, const char *msg, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start (ap,msg);
|
||||
if (message_function) message_function (num,msg,ap);
|
||||
else printMessage (num,"ODE Message",msg,ap);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
//****************************************************************************
|
||||
// windows
|
||||
|
||||
#ifdef WIN32
|
||||
|
||||
// isn't cygwin annoying!
|
||||
#ifdef CYGWIN
|
||||
#define _snprintf snprintf
|
||||
#define _vsnprintf vsnprintf
|
||||
#endif
|
||||
|
||||
|
||||
#include "windows.h"
|
||||
|
||||
|
||||
extern "C" void dError (int num, const char *msg, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start (ap,msg);
|
||||
if (error_function) error_function (num,msg,ap);
|
||||
else {
|
||||
char s[1000],title[100];
|
||||
_snprintf (title,sizeof(title),"ODE Error %d",num);
|
||||
_vsnprintf (s,sizeof(s),msg,ap);
|
||||
s[sizeof(s)-1] = 0;
|
||||
MessageBox(0,s,title,MB_OK | MB_ICONWARNING);
|
||||
}
|
||||
exit (1);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dDebug (int num, const char *msg, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start (ap,msg);
|
||||
if (debug_function) debug_function (num,msg,ap);
|
||||
else {
|
||||
char s[1000],title[100];
|
||||
_snprintf (title,sizeof(title),"ODE INTERNAL ERROR %d",num);
|
||||
_vsnprintf (s,sizeof(s),msg,ap);
|
||||
s[sizeof(s)-1] = 0;
|
||||
MessageBox(0,s,title,MB_OK | MB_ICONSTOP);
|
||||
}
|
||||
abort();
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dMessage (int num, const char *msg, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start (ap,msg);
|
||||
if (message_function) message_function (num,msg,ap);
|
||||
else printMessage (num,"ODE Message",msg,ap);
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
30
extern/ode/dist/ode/src/fastdot.c
vendored
30
extern/ode/dist/ode/src/fastdot.c
vendored
@@ -1,30 +0,0 @@
|
||||
/* generated code, do not edit. */
|
||||
|
||||
#include "ode/matrix.h"
|
||||
|
||||
|
||||
dReal dDot (const dReal *a, const dReal *b, int n)
|
||||
{
|
||||
dReal p0,q0,m0,p1,q1,m1,sum;
|
||||
sum = 0;
|
||||
n -= 2;
|
||||
while (n >= 0) {
|
||||
p0 = a[0]; q0 = b[0];
|
||||
m0 = p0 * q0;
|
||||
p1 = a[1]; q1 = b[1];
|
||||
m1 = p1 * q1;
|
||||
sum += m0;
|
||||
sum += m1;
|
||||
a += 2;
|
||||
b += 2;
|
||||
n -= 2;
|
||||
}
|
||||
n += 2;
|
||||
while (n > 0) {
|
||||
sum += (*a) * (*b);
|
||||
a++;
|
||||
b++;
|
||||
n--;
|
||||
}
|
||||
return sum;
|
||||
}
|
||||
381
extern/ode/dist/ode/src/fastldlt.c
vendored
381
extern/ode/dist/ode/src/fastldlt.c
vendored
@@ -1,381 +0,0 @@
|
||||
/* generated code, do not edit. */
|
||||
|
||||
#include "ode/matrix.h"
|
||||
|
||||
/* solve L*X=B, with B containing 1 right hand sides.
|
||||
* L is an n*n lower triangular matrix with ones on the diagonal.
|
||||
* L is stored by rows and its leading dimension is lskip.
|
||||
* B is an n*1 matrix that contains the right hand sides.
|
||||
* B is stored by columns and its leading dimension is also lskip.
|
||||
* B is overwritten with X.
|
||||
* this processes blocks of 2*2.
|
||||
* if this is in the factorizer source file, n must be a multiple of 2.
|
||||
*/
|
||||
|
||||
static void dSolveL1_1 (const dReal *L, dReal *B, int n, int lskip1)
|
||||
{
|
||||
/* declare variables - Z matrix, p and q vectors, etc */
|
||||
dReal Z11,m11,Z21,m21,p1,q1,p2,*ex;
|
||||
const dReal *ell;
|
||||
int i,j;
|
||||
/* compute all 2 x 1 blocks of X */
|
||||
for (i=0; i < n; i+=2) {
|
||||
/* compute all 2 x 1 block of X, from rows i..i+2-1 */
|
||||
/* set the Z matrix to 0 */
|
||||
Z11=0;
|
||||
Z21=0;
|
||||
ell = L + i*lskip1;
|
||||
ex = B;
|
||||
/* the inner loop that computes outer products and adds them to Z */
|
||||
for (j=i-2; j >= 0; j -= 2) {
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
p1=ell[0];
|
||||
q1=ex[0];
|
||||
m11 = p1 * q1;
|
||||
p2=ell[lskip1];
|
||||
m21 = p2 * q1;
|
||||
Z11 += m11;
|
||||
Z21 += m21;
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
p1=ell[1];
|
||||
q1=ex[1];
|
||||
m11 = p1 * q1;
|
||||
p2=ell[1+lskip1];
|
||||
m21 = p2 * q1;
|
||||
/* advance pointers */
|
||||
ell += 2;
|
||||
ex += 2;
|
||||
Z11 += m11;
|
||||
Z21 += m21;
|
||||
/* end of inner loop */
|
||||
}
|
||||
/* compute left-over iterations */
|
||||
j += 2;
|
||||
for (; j > 0; j--) {
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
p1=ell[0];
|
||||
q1=ex[0];
|
||||
m11 = p1 * q1;
|
||||
p2=ell[lskip1];
|
||||
m21 = p2 * q1;
|
||||
/* advance pointers */
|
||||
ell += 1;
|
||||
ex += 1;
|
||||
Z11 += m11;
|
||||
Z21 += m21;
|
||||
}
|
||||
/* finish computing the X(i) block */
|
||||
Z11 = ex[0] - Z11;
|
||||
ex[0] = Z11;
|
||||
p1 = ell[lskip1];
|
||||
Z21 = ex[1] - Z21 - p1*Z11;
|
||||
ex[1] = Z21;
|
||||
/* end of outer loop */
|
||||
}
|
||||
}
|
||||
|
||||
/* solve L*X=B, with B containing 2 right hand sides.
|
||||
* L is an n*n lower triangular matrix with ones on the diagonal.
|
||||
* L is stored by rows and its leading dimension is lskip.
|
||||
* B is an n*2 matrix that contains the right hand sides.
|
||||
* B is stored by columns and its leading dimension is also lskip.
|
||||
* B is overwritten with X.
|
||||
* this processes blocks of 2*2.
|
||||
* if this is in the factorizer source file, n must be a multiple of 2.
|
||||
*/
|
||||
|
||||
static void dSolveL1_2 (const dReal *L, dReal *B, int n, int lskip1)
|
||||
{
|
||||
/* declare variables - Z matrix, p and q vectors, etc */
|
||||
dReal Z11,m11,Z12,m12,Z21,m21,Z22,m22,p1,q1,p2,q2,*ex;
|
||||
const dReal *ell;
|
||||
int i,j;
|
||||
/* compute all 2 x 2 blocks of X */
|
||||
for (i=0; i < n; i+=2) {
|
||||
/* compute all 2 x 2 block of X, from rows i..i+2-1 */
|
||||
/* set the Z matrix to 0 */
|
||||
Z11=0;
|
||||
Z12=0;
|
||||
Z21=0;
|
||||
Z22=0;
|
||||
ell = L + i*lskip1;
|
||||
ex = B;
|
||||
/* the inner loop that computes outer products and adds them to Z */
|
||||
for (j=i-2; j >= 0; j -= 2) {
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
p1=ell[0];
|
||||
q1=ex[0];
|
||||
m11 = p1 * q1;
|
||||
q2=ex[lskip1];
|
||||
m12 = p1 * q2;
|
||||
p2=ell[lskip1];
|
||||
m21 = p2 * q1;
|
||||
m22 = p2 * q2;
|
||||
Z11 += m11;
|
||||
Z12 += m12;
|
||||
Z21 += m21;
|
||||
Z22 += m22;
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
p1=ell[1];
|
||||
q1=ex[1];
|
||||
m11 = p1 * q1;
|
||||
q2=ex[1+lskip1];
|
||||
m12 = p1 * q2;
|
||||
p2=ell[1+lskip1];
|
||||
m21 = p2 * q1;
|
||||
m22 = p2 * q2;
|
||||
/* advance pointers */
|
||||
ell += 2;
|
||||
ex += 2;
|
||||
Z11 += m11;
|
||||
Z12 += m12;
|
||||
Z21 += m21;
|
||||
Z22 += m22;
|
||||
/* end of inner loop */
|
||||
}
|
||||
/* compute left-over iterations */
|
||||
j += 2;
|
||||
for (; j > 0; j--) {
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
p1=ell[0];
|
||||
q1=ex[0];
|
||||
m11 = p1 * q1;
|
||||
q2=ex[lskip1];
|
||||
m12 = p1 * q2;
|
||||
p2=ell[lskip1];
|
||||
m21 = p2 * q1;
|
||||
m22 = p2 * q2;
|
||||
/* advance pointers */
|
||||
ell += 1;
|
||||
ex += 1;
|
||||
Z11 += m11;
|
||||
Z12 += m12;
|
||||
Z21 += m21;
|
||||
Z22 += m22;
|
||||
}
|
||||
/* finish computing the X(i) block */
|
||||
Z11 = ex[0] - Z11;
|
||||
ex[0] = Z11;
|
||||
Z12 = ex[lskip1] - Z12;
|
||||
ex[lskip1] = Z12;
|
||||
p1 = ell[lskip1];
|
||||
Z21 = ex[1] - Z21 - p1*Z11;
|
||||
ex[1] = Z21;
|
||||
Z22 = ex[1+lskip1] - Z22 - p1*Z12;
|
||||
ex[1+lskip1] = Z22;
|
||||
/* end of outer loop */
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dFactorLDLT (dReal *A, dReal *d, int n, int nskip1)
|
||||
{
|
||||
int i,j;
|
||||
dReal sum,*ell,*dee,dd,p1,p2,q1,q2,Z11,m11,Z21,m21,Z22,m22;
|
||||
if (n < 1) return;
|
||||
|
||||
for (i=0; i<=n-2; i += 2) {
|
||||
/* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */
|
||||
dSolveL1_2 (A,A+i*nskip1,i,nskip1);
|
||||
/* scale the elements in a 2 x i block at A(i,0), and also */
|
||||
/* compute Z = the outer product matrix that we'll need. */
|
||||
Z11 = 0;
|
||||
Z21 = 0;
|
||||
Z22 = 0;
|
||||
ell = A+i*nskip1;
|
||||
dee = d;
|
||||
for (j=i-6; j >= 0; j -= 6) {
|
||||
p1 = ell[0];
|
||||
p2 = ell[nskip1];
|
||||
dd = dee[0];
|
||||
q1 = p1*dd;
|
||||
q2 = p2*dd;
|
||||
ell[0] = q1;
|
||||
ell[nskip1] = q2;
|
||||
m11 = p1*q1;
|
||||
m21 = p2*q1;
|
||||
m22 = p2*q2;
|
||||
Z11 += m11;
|
||||
Z21 += m21;
|
||||
Z22 += m22;
|
||||
p1 = ell[1];
|
||||
p2 = ell[1+nskip1];
|
||||
dd = dee[1];
|
||||
q1 = p1*dd;
|
||||
q2 = p2*dd;
|
||||
ell[1] = q1;
|
||||
ell[1+nskip1] = q2;
|
||||
m11 = p1*q1;
|
||||
m21 = p2*q1;
|
||||
m22 = p2*q2;
|
||||
Z11 += m11;
|
||||
Z21 += m21;
|
||||
Z22 += m22;
|
||||
p1 = ell[2];
|
||||
p2 = ell[2+nskip1];
|
||||
dd = dee[2];
|
||||
q1 = p1*dd;
|
||||
q2 = p2*dd;
|
||||
ell[2] = q1;
|
||||
ell[2+nskip1] = q2;
|
||||
m11 = p1*q1;
|
||||
m21 = p2*q1;
|
||||
m22 = p2*q2;
|
||||
Z11 += m11;
|
||||
Z21 += m21;
|
||||
Z22 += m22;
|
||||
p1 = ell[3];
|
||||
p2 = ell[3+nskip1];
|
||||
dd = dee[3];
|
||||
q1 = p1*dd;
|
||||
q2 = p2*dd;
|
||||
ell[3] = q1;
|
||||
ell[3+nskip1] = q2;
|
||||
m11 = p1*q1;
|
||||
m21 = p2*q1;
|
||||
m22 = p2*q2;
|
||||
Z11 += m11;
|
||||
Z21 += m21;
|
||||
Z22 += m22;
|
||||
p1 = ell[4];
|
||||
p2 = ell[4+nskip1];
|
||||
dd = dee[4];
|
||||
q1 = p1*dd;
|
||||
q2 = p2*dd;
|
||||
ell[4] = q1;
|
||||
ell[4+nskip1] = q2;
|
||||
m11 = p1*q1;
|
||||
m21 = p2*q1;
|
||||
m22 = p2*q2;
|
||||
Z11 += m11;
|
||||
Z21 += m21;
|
||||
Z22 += m22;
|
||||
p1 = ell[5];
|
||||
p2 = ell[5+nskip1];
|
||||
dd = dee[5];
|
||||
q1 = p1*dd;
|
||||
q2 = p2*dd;
|
||||
ell[5] = q1;
|
||||
ell[5+nskip1] = q2;
|
||||
m11 = p1*q1;
|
||||
m21 = p2*q1;
|
||||
m22 = p2*q2;
|
||||
Z11 += m11;
|
||||
Z21 += m21;
|
||||
Z22 += m22;
|
||||
ell += 6;
|
||||
dee += 6;
|
||||
}
|
||||
/* compute left-over iterations */
|
||||
j += 6;
|
||||
for (; j > 0; j--) {
|
||||
p1 = ell[0];
|
||||
p2 = ell[nskip1];
|
||||
dd = dee[0];
|
||||
q1 = p1*dd;
|
||||
q2 = p2*dd;
|
||||
ell[0] = q1;
|
||||
ell[nskip1] = q2;
|
||||
m11 = p1*q1;
|
||||
m21 = p2*q1;
|
||||
m22 = p2*q2;
|
||||
Z11 += m11;
|
||||
Z21 += m21;
|
||||
Z22 += m22;
|
||||
ell++;
|
||||
dee++;
|
||||
}
|
||||
/* solve for diagonal 2 x 2 block at A(i,i) */
|
||||
Z11 = ell[0] - Z11;
|
||||
Z21 = ell[nskip1] - Z21;
|
||||
Z22 = ell[1+nskip1] - Z22;
|
||||
dee = d + i;
|
||||
/* factorize 2 x 2 block Z,dee */
|
||||
/* factorize row 1 */
|
||||
dee[0] = dRecip(Z11);
|
||||
/* factorize row 2 */
|
||||
sum = 0;
|
||||
q1 = Z21;
|
||||
q2 = q1 * dee[0];
|
||||
Z21 = q2;
|
||||
sum += q1*q2;
|
||||
dee[1] = dRecip(Z22 - sum);
|
||||
/* done factorizing 2 x 2 block */
|
||||
ell[nskip1] = Z21;
|
||||
}
|
||||
/* compute the (less than 2) rows at the bottom */
|
||||
switch (n-i) {
|
||||
case 0:
|
||||
break;
|
||||
|
||||
case 1:
|
||||
dSolveL1_1 (A,A+i*nskip1,i,nskip1);
|
||||
/* scale the elements in a 1 x i block at A(i,0), and also */
|
||||
/* compute Z = the outer product matrix that we'll need. */
|
||||
Z11 = 0;
|
||||
ell = A+i*nskip1;
|
||||
dee = d;
|
||||
for (j=i-6; j >= 0; j -= 6) {
|
||||
p1 = ell[0];
|
||||
dd = dee[0];
|
||||
q1 = p1*dd;
|
||||
ell[0] = q1;
|
||||
m11 = p1*q1;
|
||||
Z11 += m11;
|
||||
p1 = ell[1];
|
||||
dd = dee[1];
|
||||
q1 = p1*dd;
|
||||
ell[1] = q1;
|
||||
m11 = p1*q1;
|
||||
Z11 += m11;
|
||||
p1 = ell[2];
|
||||
dd = dee[2];
|
||||
q1 = p1*dd;
|
||||
ell[2] = q1;
|
||||
m11 = p1*q1;
|
||||
Z11 += m11;
|
||||
p1 = ell[3];
|
||||
dd = dee[3];
|
||||
q1 = p1*dd;
|
||||
ell[3] = q1;
|
||||
m11 = p1*q1;
|
||||
Z11 += m11;
|
||||
p1 = ell[4];
|
||||
dd = dee[4];
|
||||
q1 = p1*dd;
|
||||
ell[4] = q1;
|
||||
m11 = p1*q1;
|
||||
Z11 += m11;
|
||||
p1 = ell[5];
|
||||
dd = dee[5];
|
||||
q1 = p1*dd;
|
||||
ell[5] = q1;
|
||||
m11 = p1*q1;
|
||||
Z11 += m11;
|
||||
ell += 6;
|
||||
dee += 6;
|
||||
}
|
||||
/* compute left-over iterations */
|
||||
j += 6;
|
||||
for (; j > 0; j--) {
|
||||
p1 = ell[0];
|
||||
dd = dee[0];
|
||||
q1 = p1*dd;
|
||||
ell[0] = q1;
|
||||
m11 = p1*q1;
|
||||
Z11 += m11;
|
||||
ell++;
|
||||
dee++;
|
||||
}
|
||||
/* solve for diagonal 1 x 1 block at A(i,i) */
|
||||
Z11 = ell[0] - Z11;
|
||||
dee = d + i;
|
||||
/* factorize 1 x 1 block Z,dee */
|
||||
/* factorize row 1 */
|
||||
dee[0] = dRecip(Z11);
|
||||
/* done factorizing 1 x 1 block */
|
||||
break;
|
||||
|
||||
default: *((char*)0)=0; /* this should never happen! */
|
||||
}
|
||||
}
|
||||
298
extern/ode/dist/ode/src/fastlsolve.c
vendored
298
extern/ode/dist/ode/src/fastlsolve.c
vendored
@@ -1,298 +0,0 @@
|
||||
/* generated code, do not edit. */
|
||||
|
||||
#include "ode/matrix.h"
|
||||
|
||||
/* solve L*X=B, with B containing 1 right hand sides.
|
||||
* L is an n*n lower triangular matrix with ones on the diagonal.
|
||||
* L is stored by rows and its leading dimension is lskip.
|
||||
* B is an n*1 matrix that contains the right hand sides.
|
||||
* B is stored by columns and its leading dimension is also lskip.
|
||||
* B is overwritten with X.
|
||||
* this processes blocks of 4*4.
|
||||
* if this is in the factorizer source file, n must be a multiple of 4.
|
||||
*/
|
||||
|
||||
void dSolveL1 (const dReal *L, dReal *B, int n, int lskip1)
|
||||
{
|
||||
/* declare variables - Z matrix, p and q vectors, etc */
|
||||
dReal Z11,Z21,Z31,Z41,p1,q1,p2,p3,p4,*ex;
|
||||
const dReal *ell;
|
||||
int lskip2,lskip3,i,j;
|
||||
/* compute lskip values */
|
||||
lskip2 = 2*lskip1;
|
||||
lskip3 = 3*lskip1;
|
||||
/* compute all 4 x 1 blocks of X */
|
||||
for (i=0; i <= n-4; i+=4) {
|
||||
/* compute all 4 x 1 block of X, from rows i..i+4-1 */
|
||||
/* set the Z matrix to 0 */
|
||||
Z11=0;
|
||||
Z21=0;
|
||||
Z31=0;
|
||||
Z41=0;
|
||||
ell = L + i*lskip1;
|
||||
ex = B;
|
||||
/* the inner loop that computes outer products and adds them to Z */
|
||||
for (j=i-12; j >= 0; j -= 12) {
|
||||
/* load p and q values */
|
||||
p1=ell[0];
|
||||
q1=ex[0];
|
||||
p2=ell[lskip1];
|
||||
p3=ell[lskip2];
|
||||
p4=ell[lskip3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
Z21 += p2 * q1;
|
||||
Z31 += p3 * q1;
|
||||
Z41 += p4 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[1];
|
||||
q1=ex[1];
|
||||
p2=ell[1+lskip1];
|
||||
p3=ell[1+lskip2];
|
||||
p4=ell[1+lskip3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
Z21 += p2 * q1;
|
||||
Z31 += p3 * q1;
|
||||
Z41 += p4 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[2];
|
||||
q1=ex[2];
|
||||
p2=ell[2+lskip1];
|
||||
p3=ell[2+lskip2];
|
||||
p4=ell[2+lskip3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
Z21 += p2 * q1;
|
||||
Z31 += p3 * q1;
|
||||
Z41 += p4 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[3];
|
||||
q1=ex[3];
|
||||
p2=ell[3+lskip1];
|
||||
p3=ell[3+lskip2];
|
||||
p4=ell[3+lskip3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
Z21 += p2 * q1;
|
||||
Z31 += p3 * q1;
|
||||
Z41 += p4 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[4];
|
||||
q1=ex[4];
|
||||
p2=ell[4+lskip1];
|
||||
p3=ell[4+lskip2];
|
||||
p4=ell[4+lskip3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
Z21 += p2 * q1;
|
||||
Z31 += p3 * q1;
|
||||
Z41 += p4 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[5];
|
||||
q1=ex[5];
|
||||
p2=ell[5+lskip1];
|
||||
p3=ell[5+lskip2];
|
||||
p4=ell[5+lskip3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
Z21 += p2 * q1;
|
||||
Z31 += p3 * q1;
|
||||
Z41 += p4 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[6];
|
||||
q1=ex[6];
|
||||
p2=ell[6+lskip1];
|
||||
p3=ell[6+lskip2];
|
||||
p4=ell[6+lskip3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
Z21 += p2 * q1;
|
||||
Z31 += p3 * q1;
|
||||
Z41 += p4 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[7];
|
||||
q1=ex[7];
|
||||
p2=ell[7+lskip1];
|
||||
p3=ell[7+lskip2];
|
||||
p4=ell[7+lskip3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
Z21 += p2 * q1;
|
||||
Z31 += p3 * q1;
|
||||
Z41 += p4 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[8];
|
||||
q1=ex[8];
|
||||
p2=ell[8+lskip1];
|
||||
p3=ell[8+lskip2];
|
||||
p4=ell[8+lskip3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
Z21 += p2 * q1;
|
||||
Z31 += p3 * q1;
|
||||
Z41 += p4 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[9];
|
||||
q1=ex[9];
|
||||
p2=ell[9+lskip1];
|
||||
p3=ell[9+lskip2];
|
||||
p4=ell[9+lskip3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
Z21 += p2 * q1;
|
||||
Z31 += p3 * q1;
|
||||
Z41 += p4 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[10];
|
||||
q1=ex[10];
|
||||
p2=ell[10+lskip1];
|
||||
p3=ell[10+lskip2];
|
||||
p4=ell[10+lskip3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
Z21 += p2 * q1;
|
||||
Z31 += p3 * q1;
|
||||
Z41 += p4 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[11];
|
||||
q1=ex[11];
|
||||
p2=ell[11+lskip1];
|
||||
p3=ell[11+lskip2];
|
||||
p4=ell[11+lskip3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
Z21 += p2 * q1;
|
||||
Z31 += p3 * q1;
|
||||
Z41 += p4 * q1;
|
||||
/* advance pointers */
|
||||
ell += 12;
|
||||
ex += 12;
|
||||
/* end of inner loop */
|
||||
}
|
||||
/* compute left-over iterations */
|
||||
j += 12;
|
||||
for (; j > 0; j--) {
|
||||
/* load p and q values */
|
||||
p1=ell[0];
|
||||
q1=ex[0];
|
||||
p2=ell[lskip1];
|
||||
p3=ell[lskip2];
|
||||
p4=ell[lskip3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
Z21 += p2 * q1;
|
||||
Z31 += p3 * q1;
|
||||
Z41 += p4 * q1;
|
||||
/* advance pointers */
|
||||
ell += 1;
|
||||
ex += 1;
|
||||
}
|
||||
/* finish computing the X(i) block */
|
||||
Z11 = ex[0] - Z11;
|
||||
ex[0] = Z11;
|
||||
p1 = ell[lskip1];
|
||||
Z21 = ex[1] - Z21 - p1*Z11;
|
||||
ex[1] = Z21;
|
||||
p1 = ell[lskip2];
|
||||
p2 = ell[1+lskip2];
|
||||
Z31 = ex[2] - Z31 - p1*Z11 - p2*Z21;
|
||||
ex[2] = Z31;
|
||||
p1 = ell[lskip3];
|
||||
p2 = ell[1+lskip3];
|
||||
p3 = ell[2+lskip3];
|
||||
Z41 = ex[3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31;
|
||||
ex[3] = Z41;
|
||||
/* end of outer loop */
|
||||
}
|
||||
/* compute rows at end that are not a multiple of block size */
|
||||
for (; i < n; i++) {
|
||||
/* compute all 1 x 1 block of X, from rows i..i+1-1 */
|
||||
/* set the Z matrix to 0 */
|
||||
Z11=0;
|
||||
ell = L + i*lskip1;
|
||||
ex = B;
|
||||
/* the inner loop that computes outer products and adds them to Z */
|
||||
for (j=i-12; j >= 0; j -= 12) {
|
||||
/* load p and q values */
|
||||
p1=ell[0];
|
||||
q1=ex[0];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[1];
|
||||
q1=ex[1];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[2];
|
||||
q1=ex[2];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[3];
|
||||
q1=ex[3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[4];
|
||||
q1=ex[4];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[5];
|
||||
q1=ex[5];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[6];
|
||||
q1=ex[6];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[7];
|
||||
q1=ex[7];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[8];
|
||||
q1=ex[8];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[9];
|
||||
q1=ex[9];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[10];
|
||||
q1=ex[10];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
/* load p and q values */
|
||||
p1=ell[11];
|
||||
q1=ex[11];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
/* advance pointers */
|
||||
ell += 12;
|
||||
ex += 12;
|
||||
/* end of inner loop */
|
||||
}
|
||||
/* compute left-over iterations */
|
||||
j += 12;
|
||||
for (; j > 0; j--) {
|
||||
/* load p and q values */
|
||||
p1=ell[0];
|
||||
q1=ex[0];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
Z11 += p1 * q1;
|
||||
/* advance pointers */
|
||||
ell += 1;
|
||||
ex += 1;
|
||||
}
|
||||
/* finish computing the X(i) block */
|
||||
Z11 = ex[0] - Z11;
|
||||
ex[0] = Z11;
|
||||
}
|
||||
}
|
||||
199
extern/ode/dist/ode/src/fastltsolve.c
vendored
199
extern/ode/dist/ode/src/fastltsolve.c
vendored
@@ -1,199 +0,0 @@
|
||||
/* generated code, do not edit. */
|
||||
|
||||
#include "ode/matrix.h"
|
||||
|
||||
/* solve L^T * x=b, with b containing 1 right hand side.
|
||||
* L is an n*n lower triangular matrix with ones on the diagonal.
|
||||
* L is stored by rows and its leading dimension is lskip.
|
||||
* b is an n*1 matrix that contains the right hand side.
|
||||
* b is overwritten with x.
|
||||
* this processes blocks of 4.
|
||||
*/
|
||||
|
||||
void dSolveL1T (const dReal *L, dReal *B, int n, int lskip1)
|
||||
{
|
||||
/* declare variables - Z matrix, p and q vectors, etc */
|
||||
dReal Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex;
|
||||
const dReal *ell;
|
||||
int lskip2,lskip3,i,j;
|
||||
/* special handling for L and B because we're solving L1 *transpose* */
|
||||
L = L + (n-1)*(lskip1+1);
|
||||
B = B + n-1;
|
||||
lskip1 = -lskip1;
|
||||
/* compute lskip values */
|
||||
lskip2 = 2*lskip1;
|
||||
lskip3 = 3*lskip1;
|
||||
/* compute all 4 x 1 blocks of X */
|
||||
for (i=0; i <= n-4; i+=4) {
|
||||
/* compute all 4 x 1 block of X, from rows i..i+4-1 */
|
||||
/* set the Z matrix to 0 */
|
||||
Z11=0;
|
||||
Z21=0;
|
||||
Z31=0;
|
||||
Z41=0;
|
||||
ell = L - i;
|
||||
ex = B;
|
||||
/* the inner loop that computes outer products and adds them to Z */
|
||||
for (j=i-4; j >= 0; j -= 4) {
|
||||
/* load p and q values */
|
||||
p1=ell[0];
|
||||
q1=ex[0];
|
||||
p2=ell[-1];
|
||||
p3=ell[-2];
|
||||
p4=ell[-3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
m11 = p1 * q1;
|
||||
m21 = p2 * q1;
|
||||
m31 = p3 * q1;
|
||||
m41 = p4 * q1;
|
||||
ell += lskip1;
|
||||
Z11 += m11;
|
||||
Z21 += m21;
|
||||
Z31 += m31;
|
||||
Z41 += m41;
|
||||
/* load p and q values */
|
||||
p1=ell[0];
|
||||
q1=ex[-1];
|
||||
p2=ell[-1];
|
||||
p3=ell[-2];
|
||||
p4=ell[-3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
m11 = p1 * q1;
|
||||
m21 = p2 * q1;
|
||||
m31 = p3 * q1;
|
||||
m41 = p4 * q1;
|
||||
ell += lskip1;
|
||||
Z11 += m11;
|
||||
Z21 += m21;
|
||||
Z31 += m31;
|
||||
Z41 += m41;
|
||||
/* load p and q values */
|
||||
p1=ell[0];
|
||||
q1=ex[-2];
|
||||
p2=ell[-1];
|
||||
p3=ell[-2];
|
||||
p4=ell[-3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
m11 = p1 * q1;
|
||||
m21 = p2 * q1;
|
||||
m31 = p3 * q1;
|
||||
m41 = p4 * q1;
|
||||
ell += lskip1;
|
||||
Z11 += m11;
|
||||
Z21 += m21;
|
||||
Z31 += m31;
|
||||
Z41 += m41;
|
||||
/* load p and q values */
|
||||
p1=ell[0];
|
||||
q1=ex[-3];
|
||||
p2=ell[-1];
|
||||
p3=ell[-2];
|
||||
p4=ell[-3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
m11 = p1 * q1;
|
||||
m21 = p2 * q1;
|
||||
m31 = p3 * q1;
|
||||
m41 = p4 * q1;
|
||||
ell += lskip1;
|
||||
ex -= 4;
|
||||
Z11 += m11;
|
||||
Z21 += m21;
|
||||
Z31 += m31;
|
||||
Z41 += m41;
|
||||
/* end of inner loop */
|
||||
}
|
||||
/* compute left-over iterations */
|
||||
j += 4;
|
||||
for (; j > 0; j--) {
|
||||
/* load p and q values */
|
||||
p1=ell[0];
|
||||
q1=ex[0];
|
||||
p2=ell[-1];
|
||||
p3=ell[-2];
|
||||
p4=ell[-3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
m11 = p1 * q1;
|
||||
m21 = p2 * q1;
|
||||
m31 = p3 * q1;
|
||||
m41 = p4 * q1;
|
||||
ell += lskip1;
|
||||
ex -= 1;
|
||||
Z11 += m11;
|
||||
Z21 += m21;
|
||||
Z31 += m31;
|
||||
Z41 += m41;
|
||||
}
|
||||
/* finish computing the X(i) block */
|
||||
Z11 = ex[0] - Z11;
|
||||
ex[0] = Z11;
|
||||
p1 = ell[-1];
|
||||
Z21 = ex[-1] - Z21 - p1*Z11;
|
||||
ex[-1] = Z21;
|
||||
p1 = ell[-2];
|
||||
p2 = ell[-2+lskip1];
|
||||
Z31 = ex[-2] - Z31 - p1*Z11 - p2*Z21;
|
||||
ex[-2] = Z31;
|
||||
p1 = ell[-3];
|
||||
p2 = ell[-3+lskip1];
|
||||
p3 = ell[-3+lskip2];
|
||||
Z41 = ex[-3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31;
|
||||
ex[-3] = Z41;
|
||||
/* end of outer loop */
|
||||
}
|
||||
/* compute rows at end that are not a multiple of block size */
|
||||
for (; i < n; i++) {
|
||||
/* compute all 1 x 1 block of X, from rows i..i+1-1 */
|
||||
/* set the Z matrix to 0 */
|
||||
Z11=0;
|
||||
ell = L - i;
|
||||
ex = B;
|
||||
/* the inner loop that computes outer products and adds them to Z */
|
||||
for (j=i-4; j >= 0; j -= 4) {
|
||||
/* load p and q values */
|
||||
p1=ell[0];
|
||||
q1=ex[0];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
m11 = p1 * q1;
|
||||
ell += lskip1;
|
||||
Z11 += m11;
|
||||
/* load p and q values */
|
||||
p1=ell[0];
|
||||
q1=ex[-1];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
m11 = p1 * q1;
|
||||
ell += lskip1;
|
||||
Z11 += m11;
|
||||
/* load p and q values */
|
||||
p1=ell[0];
|
||||
q1=ex[-2];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
m11 = p1 * q1;
|
||||
ell += lskip1;
|
||||
Z11 += m11;
|
||||
/* load p and q values */
|
||||
p1=ell[0];
|
||||
q1=ex[-3];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
m11 = p1 * q1;
|
||||
ell += lskip1;
|
||||
ex -= 4;
|
||||
Z11 += m11;
|
||||
/* end of inner loop */
|
||||
}
|
||||
/* compute left-over iterations */
|
||||
j += 4;
|
||||
for (; j > 0; j--) {
|
||||
/* load p and q values */
|
||||
p1=ell[0];
|
||||
q1=ex[0];
|
||||
/* compute outer product and add it to the Z matrix */
|
||||
m11 = p1 * q1;
|
||||
ell += lskip1;
|
||||
ex -= 1;
|
||||
Z11 += m11;
|
||||
}
|
||||
/* finish computing the X(i) block */
|
||||
Z11 = ex[0] - Z11;
|
||||
ex[0] = Z11;
|
||||
}
|
||||
}
|
||||
2207
extern/ode/dist/ode/src/geom.cpp
vendored
2207
extern/ode/dist/ode/src/geom.cpp
vendored
@@ -1,2207 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
/*
|
||||
|
||||
the rule is that only the low level primitive collision functions should set
|
||||
dContactGeom::g1 and dContactGeom::g2.
|
||||
|
||||
*/
|
||||
|
||||
#define SHARED_GEOM_H_INCLUDED_FROM_DEFINING_FILE 1
|
||||
#include <ode/common.h>
|
||||
#include <ode/geom.h>
|
||||
#include <ode/rotation.h>
|
||||
#include <ode/odemath.h>
|
||||
#include <ode/memory.h>
|
||||
#include <ode/misc.h>
|
||||
#include <ode/objects.h>
|
||||
#include <ode/matrix.h>
|
||||
#include "objects.h"
|
||||
#include "array.h"
|
||||
#include "geom_internal.h"
|
||||
|
||||
//****************************************************************************
|
||||
// collision utilities.
|
||||
|
||||
// given a pointer `p' to a dContactGeom, return the dContactGeom at
|
||||
// p + skip bytes.
|
||||
|
||||
#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
|
||||
|
||||
|
||||
// if the spheres (p1,r1) and (p2,r2) collide, set the contact `c' and
|
||||
// return 1, else return 0.
|
||||
|
||||
static int dCollideSpheres (dVector3 p1, dReal r1,
|
||||
dVector3 p2, dReal r2, dContactGeom *c)
|
||||
{
|
||||
// printf ("d=%.2f (%.2f %.2f %.2f) (%.2f %.2f %.2f) r1=%.2f r2=%.2f\n",
|
||||
// d,p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],r1,r2);
|
||||
|
||||
dReal d = dDISTANCE (p1,p2);
|
||||
if (d > (r1 + r2)) return 0;
|
||||
if (d <= 0) {
|
||||
c->pos[0] = p1[0];
|
||||
c->pos[1] = p1[1];
|
||||
c->pos[2] = p1[2];
|
||||
c->normal[0] = 1;
|
||||
c->normal[1] = 0;
|
||||
c->normal[2] = 0;
|
||||
c->depth = r1 + r2;
|
||||
}
|
||||
else {
|
||||
dReal d1 = dRecip (d);
|
||||
c->normal[0] = (p1[0]-p2[0])*d1;
|
||||
c->normal[1] = (p1[1]-p2[1])*d1;
|
||||
c->normal[2] = (p1[2]-p2[2])*d1;
|
||||
dReal k = REAL(0.5) * (r2 - r1 - d);
|
||||
c->pos[0] = p1[0] + c->normal[0]*k;
|
||||
c->pos[1] = p1[1] + c->normal[1]*k;
|
||||
c->pos[2] = p1[2] + c->normal[2]*k;
|
||||
c->depth = r1 + r2 - d;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
// given two lines
|
||||
// qa = pa + alpha* ua
|
||||
// qb = pb + beta * ub
|
||||
// where pa,pb are two points, ua,ub are two unit length vectors, and alpha,
|
||||
// beta go from [-inf,inf], return alpha and beta such that qa and qb are
|
||||
// as close as possible
|
||||
|
||||
static void lineClosestApproach (const dVector3 pa, const dVector3 ua,
|
||||
const dVector3 pb, const dVector3 ub,
|
||||
dReal *alpha, dReal *beta)
|
||||
{
|
||||
dVector3 p;
|
||||
p[0] = pb[0] - pa[0];
|
||||
p[1] = pb[1] - pa[1];
|
||||
p[2] = pb[2] - pa[2];
|
||||
dReal uaub = dDOT(ua,ub);
|
||||
dReal q1 = dDOT(ua,p);
|
||||
dReal q2 = -dDOT(ub,p);
|
||||
dReal d = 1-uaub*uaub;
|
||||
if (d <= 0) {
|
||||
// @@@ this needs to be made more robust
|
||||
*alpha = 0;
|
||||
*beta = 0;
|
||||
}
|
||||
else {
|
||||
d = dRecip(d);
|
||||
*alpha = (q1 + uaub*q2)*d;
|
||||
*beta = (uaub*q1 + q2)*d;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// given two line segments A and B with endpoints a1-a2 and b1-b2, return the
|
||||
// points on A and B that are closest to each other (in cp1 and cp2).
|
||||
// in the case of parallel lines where there are multiple solutions, a
|
||||
// solution involving the endpoint of at least one line will be returned.
|
||||
// this will work correctly for zero length lines, e.g. if a1==a2 and/or
|
||||
// b1==b2.
|
||||
//
|
||||
// the algorithm works by applying the voronoi clipping rule to the features
|
||||
// of the line segments. the three features of each line segment are the two
|
||||
// endpoints and the line between them. the voronoi clipping rule states that,
|
||||
// for feature X on line A and feature Y on line B, the closest points PA and
|
||||
// PB between X and Y are globally the closest points if PA is in V(Y) and
|
||||
// PB is in V(X), where V(X) is the voronoi region of X.
|
||||
|
||||
|
||||
void dClosestLineSegmentPoints (dVector3 const a1, dVector3 const a2,
|
||||
dVector3 const b1, dVector3 const b2,
|
||||
dVector3 cp1, dVector3 cp2)
|
||||
{
|
||||
dVector3 a1a2,b1b2,a1b1,a1b2,a2b1,a2b2,n;
|
||||
dReal la,lb,k,da1,da2,da3,da4,db1,db2,db3,db4,det;
|
||||
|
||||
#define SET2(a,b) a[0]=b[0]; a[1]=b[1]; a[2]=b[2];
|
||||
#define SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2];
|
||||
|
||||
// check vertex-vertex features
|
||||
|
||||
SET3 (a1a2,a2,-,a1);
|
||||
SET3 (b1b2,b2,-,b1);
|
||||
SET3 (a1b1,b1,-,a1);
|
||||
da1 = dDOT(a1a2,a1b1);
|
||||
db1 = dDOT(b1b2,a1b1);
|
||||
if (da1 <= 0 && db1 >= 0) {
|
||||
SET2 (cp1,a1);
|
||||
SET2 (cp2,b1);
|
||||
return;
|
||||
}
|
||||
|
||||
SET3 (a1b2,b2,-,a1);
|
||||
da2 = dDOT(a1a2,a1b2);
|
||||
db2 = dDOT(b1b2,a1b2);
|
||||
if (da2 <= 0 && db2 <= 0) {
|
||||
SET2 (cp1,a1);
|
||||
SET2 (cp2,b2);
|
||||
return;
|
||||
}
|
||||
|
||||
SET3 (a2b1,b1,-,a2);
|
||||
da3 = dDOT(a1a2,a2b1);
|
||||
db3 = dDOT(b1b2,a2b1);
|
||||
if (da3 >= 0 && db3 >= 0) {
|
||||
SET2 (cp1,a2);
|
||||
SET2 (cp2,b1);
|
||||
return;
|
||||
}
|
||||
|
||||
SET3 (a2b2,b2,-,a2);
|
||||
da4 = dDOT(a1a2,a2b2);
|
||||
db4 = dDOT(b1b2,a2b2);
|
||||
if (da4 >= 0 && db4 <= 0) {
|
||||
SET2 (cp1,a2);
|
||||
SET2 (cp2,b2);
|
||||
return;
|
||||
}
|
||||
|
||||
// check edge-vertex features.
|
||||
// if one or both of the lines has zero length, we will never get to here,
|
||||
// so we do not have to worry about the following divisions by zero.
|
||||
|
||||
la = dDOT(a1a2,a1a2);
|
||||
if (da1 >= 0 && da3 <= 0) {
|
||||
k = da1 / la;
|
||||
SET3 (n,a1b1,-,k*a1a2);
|
||||
if (dDOT(b1b2,n) >= 0) {
|
||||
SET3 (cp1,a1,+,k*a1a2);
|
||||
SET2 (cp2,b1);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (da2 >= 0 && da4 <= 0) {
|
||||
k = da2 / la;
|
||||
SET3 (n,a1b2,-,k*a1a2);
|
||||
if (dDOT(b1b2,n) <= 0) {
|
||||
SET3 (cp1,a1,+,k*a1a2);
|
||||
SET2 (cp2,b2);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
lb = dDOT(b1b2,b1b2);
|
||||
if (db1 <= 0 && db2 >= 0) {
|
||||
k = -db1 / lb;
|
||||
SET3 (n,-a1b1,-,k*b1b2);
|
||||
if (dDOT(a1a2,n) >= 0) {
|
||||
SET2 (cp1,a1);
|
||||
SET3 (cp2,b1,+,k*b1b2);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (db3 <= 0 && db4 >= 0) {
|
||||
k = -db3 / lb;
|
||||
SET3 (n,-a2b1,-,k*b1b2);
|
||||
if (dDOT(a1a2,n) <= 0) {
|
||||
SET2 (cp1,a2);
|
||||
SET3 (cp2,b1,+,k*b1b2);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// it must be edge-edge
|
||||
|
||||
k = dDOT(a1a2,b1b2);
|
||||
det = la*lb - k*k;
|
||||
if (det <= 0) {
|
||||
// this should never happen, but just in case...
|
||||
SET2(cp1,a1);
|
||||
SET2(cp2,b1);
|
||||
return;
|
||||
}
|
||||
det = dRecip (det);
|
||||
dReal alpha = (lb*da1 - k*db1) * det;
|
||||
dReal beta = ( k*da1 - la*db1) * det;
|
||||
SET3 (cp1,a1,+,alpha*a1a2);
|
||||
SET3 (cp2,b1,+,beta*b1b2);
|
||||
|
||||
# undef SET2
|
||||
# undef SET3
|
||||
}
|
||||
|
||||
|
||||
// given a line segment p1-p2 and a box (center 'c', rotation 'R', side length
|
||||
// vector 'side'), compute the points of closest approach between the box
|
||||
// and the line. return these points in 'lret' (the point on the line) and
|
||||
// 'bret' (the point on the box). if the line actually penetrates the box
|
||||
// then the solution is not unique, but only one solution will be returned.
|
||||
// in this case the solution points will coincide.
|
||||
//
|
||||
// a simple root finding algorithm is used to find the value of 't' that
|
||||
// satisfies:
|
||||
// d|D(t)|^2/dt = 0
|
||||
// where:
|
||||
// |D(t)| = |p(t)-b(t)|
|
||||
// where p(t) is a point on the line parameterized by t:
|
||||
// p(t) = p1 + t*(p2-p1)
|
||||
// and b(t) is that same point clipped to the boundary of the box. in box-
|
||||
// relative coordinates d|D(t)|^2/dt is the sum of three x,y,z components
|
||||
// each of which looks like this:
|
||||
//
|
||||
// t_lo /
|
||||
// ______/ -->t
|
||||
// / t_hi
|
||||
// /
|
||||
//
|
||||
// t_lo and t_hi are the t values where the line passes through the planes
|
||||
// corresponding to the sides of the box. the algorithm computes d|D(t)|^2/dt
|
||||
// in a piecewise fashion from t=0 to t=1, stopping at the point where
|
||||
// d|D(t)|^2/dt crosses from negative to positive.
|
||||
|
||||
static void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2,
|
||||
const dVector3 c, const dMatrix3 R,
|
||||
const dVector3 side,
|
||||
dVector3 lret, dVector3 bret)
|
||||
{
|
||||
int i;
|
||||
|
||||
// compute the start and delta of the line p1-p2 relative to the box.
|
||||
// we will do all subsequent computations in this box-relative coordinate
|
||||
// system. we have to do a translation and rotation for each point.
|
||||
dVector3 tmp,s,v;
|
||||
tmp[0] = p1[0] - c[0];
|
||||
tmp[1] = p1[1] - c[1];
|
||||
tmp[2] = p1[2] - c[2];
|
||||
dMULTIPLY1_331 (s,R,tmp);
|
||||
tmp[0] = p2[0] - p1[0];
|
||||
tmp[1] = p2[1] - p1[1];
|
||||
tmp[2] = p2[2] - p1[2];
|
||||
dMULTIPLY1_331 (v,R,tmp);
|
||||
|
||||
// mirror the line so that v has all components >= 0
|
||||
dVector3 sign;
|
||||
for (i=0; i<3; i++) {
|
||||
if (v[i] < 0) {
|
||||
s[i] = -s[i];
|
||||
v[i] = -v[i];
|
||||
sign[i] = -1;
|
||||
}
|
||||
else sign[i] = 1;
|
||||
}
|
||||
|
||||
// compute v^2
|
||||
dVector3 v2;
|
||||
v2[0] = v[0]*v[0];
|
||||
v2[1] = v[1]*v[1];
|
||||
v2[2] = v[2]*v[2];
|
||||
|
||||
// compute the half-sides of the box
|
||||
dReal h[3];
|
||||
h[0] = REAL(0.5) * side[0];
|
||||
h[1] = REAL(0.5) * side[1];
|
||||
h[2] = REAL(0.5) * side[2];
|
||||
|
||||
// region is -1,0,+1 depending on which side of the box planes each
|
||||
// coordinate is on. tanchor in the next t value at which there is a
|
||||
// transition, or the last one if there are no more.
|
||||
int region[3];
|
||||
dReal tanchor[3];
|
||||
|
||||
// find the region and tanchor values for p1
|
||||
for (i=0; i<3; i++) {
|
||||
if (v[i] > 0) {
|
||||
if (s[i] < -h[i]) {
|
||||
region[i] = -1;
|
||||
tanchor[i] = (-h[i]-s[i])/v[i];
|
||||
}
|
||||
else {
|
||||
region[i] = (s[i] > h[i]);
|
||||
tanchor[i] = (h[i]-s[i])/v[i];
|
||||
}
|
||||
}
|
||||
else {
|
||||
region[i] = 0;
|
||||
tanchor[i] = 2; // this will never be a valid tanchor
|
||||
}
|
||||
}
|
||||
|
||||
// compute d|d|^2/dt for t=0. if it's >= 0 then p1 is the closest point
|
||||
dReal t=0;
|
||||
dReal dd2dt = 0;
|
||||
for (i=0; i<3; i++) dd2dt -= (region[i] ? v2[i] : 0) * tanchor[i];
|
||||
if (dd2dt >= 0) goto got_answer;
|
||||
|
||||
do {
|
||||
// find the point on the line that is at the next clip plane boundary
|
||||
dReal next_t = 1;
|
||||
for (i=0; i<3; i++) {
|
||||
if (tanchor[i] > t && tanchor[i] < 1 && tanchor[i] < next_t)
|
||||
next_t = tanchor[i];
|
||||
}
|
||||
|
||||
// compute d|d|^2/dt for the next t
|
||||
dReal next_dd2dt = 0;
|
||||
for (i=0; i<3; i++) {
|
||||
next_dd2dt += (region[i] ? v2[i] : 0) * (next_t - tanchor[i]);
|
||||
}
|
||||
|
||||
// if the sign of d|d|^2/dt has changed, solution = the crossover point
|
||||
if (next_dd2dt >= 0) {
|
||||
dReal m = (next_dd2dt-dd2dt)/(next_t - t);
|
||||
t -= dd2dt/m;
|
||||
goto got_answer;
|
||||
}
|
||||
|
||||
// advance to the next anchor point / region
|
||||
for (i=0; i<3; i++) {
|
||||
if (tanchor[i] == next_t) {
|
||||
tanchor[i] = (h[i]-s[i])/v[i];
|
||||
region[i]++;
|
||||
}
|
||||
}
|
||||
t = next_t;
|
||||
dd2dt = next_dd2dt;
|
||||
}
|
||||
while (t < 1);
|
||||
t = 1;
|
||||
|
||||
got_answer:
|
||||
|
||||
// compute closest point on the line
|
||||
for (i=0; i<3; i++) lret[i] = p1[i] + t*tmp[i]; // note: tmp=p2-p1
|
||||
|
||||
// compute closest point on the box
|
||||
for (i=0; i<3; i++) {
|
||||
tmp[i] = sign[i] * (s[i] + t*v[i]);
|
||||
if (tmp[i] < -h[i]) tmp[i] = -h[i];
|
||||
else if (tmp[i] > h[i]) tmp[i] = h[i];
|
||||
}
|
||||
dMULTIPLY0_331 (s,R,tmp);
|
||||
for (i=0; i<3; i++) bret[i] = s[i] + c[i];
|
||||
}
|
||||
|
||||
|
||||
// given a box (R,side), `R' is the rotation matrix for the box, and `side'
|
||||
// is a vector of x/y/z side lengths, return the size of the interval of the
|
||||
// box projected along the given axis. if the axis has unit length then the
|
||||
// return value will be the actual diameter, otherwise the result will be
|
||||
// scaled by the axis length.
|
||||
|
||||
static inline dReal boxDiameter (const dMatrix3 R, const dVector3 side,
|
||||
const dVector3 axis)
|
||||
{
|
||||
dVector3 q;
|
||||
dMULTIPLY1_331 (q,R,axis); // transform axis to body-relative
|
||||
return dFabs(q[0])*side[0] + dFabs(q[1])*side[1] + dFabs(q[2])*side[2];
|
||||
}
|
||||
|
||||
|
||||
// given boxes (p1,R1,side1) and (p1,R1,side1), return 1 if they intersect
|
||||
// or 0 if not.
|
||||
|
||||
int dBoxTouchesBox (const dVector3 p1, const dMatrix3 R1,
|
||||
const dVector3 side1, const dVector3 p2,
|
||||
const dMatrix3 R2, const dVector3 side2)
|
||||
{
|
||||
// two boxes are disjoint if (and only if) there is a separating axis
|
||||
// perpendicular to a face from one box or perpendicular to an edge from
|
||||
// either box. the following tests are derived from:
|
||||
// "OBB Tree: A Hierarchical Structure for Rapid Interference Detection",
|
||||
// S.Gottschalk, M.C.Lin, D.Manocha., Proc of ACM Siggraph 1996.
|
||||
|
||||
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2.
|
||||
// Qij is abs(Rij)
|
||||
dVector3 p,pp;
|
||||
dReal A1,A2,A3,B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33,
|
||||
Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33;
|
||||
|
||||
// get vector from centers of box 1 to box 2, relative to box 1
|
||||
p[0] = p2[0] - p1[0];
|
||||
p[1] = p2[1] - p1[1];
|
||||
p[2] = p2[2] - p1[2];
|
||||
dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
|
||||
|
||||
// get side lengths / 2
|
||||
A1 = side1[0]*REAL(0.5); A2 = side1[1]*REAL(0.5); A3 = side1[2]*REAL(0.5);
|
||||
B1 = side2[0]*REAL(0.5); B2 = side2[1]*REAL(0.5); B3 = side2[2]*REAL(0.5);
|
||||
|
||||
// for the following tests, excluding computation of Rij, in the worst case,
|
||||
// 15 compares, 60 adds, 81 multiplies, and 24 absolutes.
|
||||
// notation: R1=[u1 u2 u3], R2=[v1 v2 v3]
|
||||
|
||||
// separating axis = u1,u2,u3
|
||||
R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
|
||||
Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
|
||||
if (dFabs(pp[0]) > (A1 + B1*Q11 + B2*Q12 + B3*Q13)) return 0;
|
||||
R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
|
||||
Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
|
||||
if (dFabs(pp[1]) > (A2 + B1*Q21 + B2*Q22 + B3*Q23)) return 0;
|
||||
R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
|
||||
Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);
|
||||
if (dFabs(pp[2]) > (A3 + B1*Q31 + B2*Q32 + B3*Q33)) return 0;
|
||||
|
||||
// separating axis = v1,v2,v3
|
||||
if (dFabs(dDOT41(R2+0,p)) > (A1*Q11 + A2*Q21 + A3*Q31 + B1)) return 0;
|
||||
if (dFabs(dDOT41(R2+1,p)) > (A1*Q12 + A2*Q22 + A3*Q32 + B2)) return 0;
|
||||
if (dFabs(dDOT41(R2+2,p)) > (A1*Q13 + A2*Q23 + A3*Q33 + B3)) return 0;
|
||||
|
||||
// separating axis = u1 x (v1,v2,v3)
|
||||
if (dFabs(pp[2]*R21-pp[1]*R31) > A2*Q31 + A3*Q21 + B2*Q13 + B3*Q12) return 0;
|
||||
if (dFabs(pp[2]*R22-pp[1]*R32) > A2*Q32 + A3*Q22 + B1*Q13 + B3*Q11) return 0;
|
||||
if (dFabs(pp[2]*R23-pp[1]*R33) > A2*Q33 + A3*Q23 + B1*Q12 + B2*Q11) return 0;
|
||||
|
||||
// separating axis = u2 x (v1,v2,v3)
|
||||
if (dFabs(pp[0]*R31-pp[2]*R11) > A1*Q31 + A3*Q11 + B2*Q23 + B3*Q22) return 0;
|
||||
if (dFabs(pp[0]*R32-pp[2]*R12) > A1*Q32 + A3*Q12 + B1*Q23 + B3*Q21) return 0;
|
||||
if (dFabs(pp[0]*R33-pp[2]*R13) > A1*Q33 + A3*Q13 + B1*Q22 + B2*Q21) return 0;
|
||||
|
||||
// separating axis = u3 x (v1,v2,v3)
|
||||
if (dFabs(pp[1]*R11-pp[0]*R21) > A1*Q21 + A2*Q11 + B2*Q33 + B3*Q32) return 0;
|
||||
if (dFabs(pp[1]*R12-pp[0]*R22) > A1*Q22 + A2*Q12 + B1*Q33 + B3*Q31) return 0;
|
||||
if (dFabs(pp[1]*R13-pp[0]*R23) > A1*Q23 + A2*Q13 + B1*Q32 + B2*Q31) return 0;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
|
||||
// generate contact points. this returns 0 if there is no contact otherwise
|
||||
// it returns the number of contacts generated.
|
||||
// `normal' returns the contact normal.
|
||||
// `depth' returns the maximum penetration depth along that normal.
|
||||
// `code' returns a number indicating the type of contact that was detected:
|
||||
// 1,2,3 = box 2 intersects with a face of box 1
|
||||
// 4,5,6 = box 1 intersects with a face of box 2
|
||||
// 7..15 = edge-edge contact
|
||||
// `maxc' is the maximum number of contacts allowed to be generated, i.e.
|
||||
// the size of the `contact' array.
|
||||
// `contact' and `skip' are the contact array information provided to the
|
||||
// collision functions. this function only fills in the position and depth
|
||||
// fields.
|
||||
//
|
||||
// @@@ some stuff to optimize here, reuse code in contact point calculations.
|
||||
|
||||
extern "C" int dBoxBox (const dVector3 p1, const dMatrix3 R1,
|
||||
const dVector3 side1, const dVector3 p2,
|
||||
const dMatrix3 R2, const dVector3 side2,
|
||||
dVector3 normal, dReal *depth, int *code,
|
||||
int maxc, dContactGeom *contact, int skip)
|
||||
{
|
||||
dVector3 p,pp,normalC;
|
||||
const dReal *normalR = 0;
|
||||
dReal A1,A2,A3,B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33,
|
||||
Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
|
||||
int i,invert_normal;
|
||||
|
||||
// get vector from centers of box 1 to box 2, relative to box 1
|
||||
p[0] = p2[0] - p1[0];
|
||||
p[1] = p2[1] - p1[1];
|
||||
p[2] = p2[2] - p1[2];
|
||||
dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
|
||||
|
||||
// get side lengths / 2
|
||||
A1 = side1[0]*REAL(0.5); A2 = side1[1]*REAL(0.5); A3 = side1[2]*REAL(0.5);
|
||||
B1 = side2[0]*REAL(0.5); B2 = side2[1]*REAL(0.5); B3 = side2[2]*REAL(0.5);
|
||||
|
||||
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
|
||||
R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
|
||||
R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
|
||||
R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
|
||||
|
||||
Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
|
||||
Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
|
||||
Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);
|
||||
|
||||
// for all 15 possible separating axes:
|
||||
// * see if the axis separates the boxes. if so, return 0.
|
||||
// * find the depth of the penetration along the separating axis (s2)
|
||||
// * if this is the largest depth so far, record it.
|
||||
// the normal vector will be set to the separating axis with the smallest
|
||||
// depth. note: normalR is set to point to a column of R1 or R2 if that is
|
||||
// the smallest depth normal so far. otherwise normalR is 0 and normalC is
|
||||
// set to a vector relative to body 1. invert_normal is 1 if the sign of
|
||||
// the normal should be flipped.
|
||||
|
||||
#define TEST(expr1,expr2,norm,cc) \
|
||||
s2 = dFabs(expr1) - (expr2); \
|
||||
if (s2 > 0) return 0; \
|
||||
if (s2 > s) { \
|
||||
s = s2; \
|
||||
normalR = norm; \
|
||||
invert_normal = ((expr1) < 0); \
|
||||
*code = (cc); \
|
||||
}
|
||||
|
||||
s = -dInfinity;
|
||||
invert_normal = 0;
|
||||
*code = 0;
|
||||
|
||||
// separating axis = u1,u2,u3
|
||||
TEST (pp[0],(A1 + B1*Q11 + B2*Q12 + B3*Q13),R1+0,1);
|
||||
TEST (pp[1],(A2 + B1*Q21 + B2*Q22 + B3*Q23),R1+1,2);
|
||||
TEST (pp[2],(A3 + B1*Q31 + B2*Q32 + B3*Q33),R1+2,3);
|
||||
|
||||
// separating axis = v1,v2,v3
|
||||
TEST (dDOT41(R2+0,p),(A1*Q11 + A2*Q21 + A3*Q31 + B1),R2+0,4);
|
||||
TEST (dDOT41(R2+1,p),(A1*Q12 + A2*Q22 + A3*Q32 + B2),R2+1,5);
|
||||
TEST (dDOT41(R2+2,p),(A1*Q13 + A2*Q23 + A3*Q33 + B3),R2+2,6);
|
||||
|
||||
// note: cross product axes need to be scaled when s is computed.
|
||||
// normal (n1,n2,n3) is relative to box 1.
|
||||
#undef TEST
|
||||
#define TEST(expr1,expr2,n1,n2,n3,cc) \
|
||||
s2 = dFabs(expr1) - (expr2); \
|
||||
if (s2 > 0) return 0; \
|
||||
l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
|
||||
if (l > 0) { \
|
||||
s2 /= l; \
|
||||
if (s2 > s) { \
|
||||
s = s2; \
|
||||
normalR = 0; \
|
||||
normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
|
||||
invert_normal = ((expr1) < 0); \
|
||||
*code = (cc); \
|
||||
} \
|
||||
}
|
||||
|
||||
// separating axis = u1 x (v1,v2,v3)
|
||||
TEST(pp[2]*R21-pp[1]*R31,(A2*Q31+A3*Q21+B2*Q13+B3*Q12),0,-R31,R21,7);
|
||||
TEST(pp[2]*R22-pp[1]*R32,(A2*Q32+A3*Q22+B1*Q13+B3*Q11),0,-R32,R22,8);
|
||||
TEST(pp[2]*R23-pp[1]*R33,(A2*Q33+A3*Q23+B1*Q12+B2*Q11),0,-R33,R23,9);
|
||||
|
||||
// separating axis = u2 x (v1,v2,v3)
|
||||
TEST(pp[0]*R31-pp[2]*R11,(A1*Q31+A3*Q11+B2*Q23+B3*Q22),R31,0,-R11,10);
|
||||
TEST(pp[0]*R32-pp[2]*R12,(A1*Q32+A3*Q12+B1*Q23+B3*Q21),R32,0,-R12,11);
|
||||
TEST(pp[0]*R33-pp[2]*R13,(A1*Q33+A3*Q13+B1*Q22+B2*Q21),R33,0,-R13,12);
|
||||
|
||||
// separating axis = u3 x (v1,v2,v3)
|
||||
TEST(pp[1]*R11-pp[0]*R21,(A1*Q21+A2*Q11+B2*Q33+B3*Q32),-R21,R11,0,13);
|
||||
TEST(pp[1]*R12-pp[0]*R22,(A1*Q22+A2*Q12+B1*Q33+B3*Q31),-R22,R12,0,14);
|
||||
TEST(pp[1]*R13-pp[0]*R23,(A1*Q23+A2*Q13+B1*Q32+B2*Q31),-R23,R13,0,15);
|
||||
|
||||
#undef TEST
|
||||
|
||||
// if we get to this point, the boxes interpenetrate. compute the normal
|
||||
// in global coordinates.
|
||||
if (normalR) {
|
||||
normal[0] = normalR[0];
|
||||
normal[1] = normalR[4];
|
||||
normal[2] = normalR[8];
|
||||
}
|
||||
else {
|
||||
dMULTIPLY0_331 (normal,R1,normalC);
|
||||
}
|
||||
if (invert_normal) {
|
||||
normal[0] = -normal[0];
|
||||
normal[1] = -normal[1];
|
||||
normal[2] = -normal[2];
|
||||
}
|
||||
*depth = -s;
|
||||
|
||||
// compute contact point(s)
|
||||
|
||||
if (*code > 6) {
|
||||
// an edge from box 1 touches an edge from box 2.
|
||||
// find a point pa on the intersecting edge of box 1
|
||||
dVector3 pa;
|
||||
dReal sign;
|
||||
for (i=0; i<3; i++) pa[i] = p1[i];
|
||||
sign = (dDOT14(normal,R1+0) > 0) ? REAL(1.0) : REAL(-1.0);
|
||||
for (i=0; i<3; i++) pa[i] += sign * A1 * R1[i*4];
|
||||
sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
|
||||
for (i=0; i<3; i++) pa[i] += sign * A2 * R1[i*4+1];
|
||||
sign = (dDOT14(normal,R1+2) > 0) ? REAL(1.0) : REAL(-1.0);
|
||||
for (i=0; i<3; i++) pa[i] += sign * A3 * R1[i*4+2];
|
||||
|
||||
// find a point pb on the intersecting edge of box 2
|
||||
dVector3 pb;
|
||||
for (i=0; i<3; i++) pb[i] = p2[i];
|
||||
sign = (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0);
|
||||
for (i=0; i<3; i++) pb[i] += sign * B1 * R2[i*4];
|
||||
sign = (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0);
|
||||
for (i=0; i<3; i++) pb[i] += sign * B2 * R2[i*4+1];
|
||||
sign = (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0);
|
||||
for (i=0; i<3; i++) pb[i] += sign * B3 * R2[i*4+2];
|
||||
|
||||
dReal alpha,beta;
|
||||
dVector3 ua,ub;
|
||||
for (i=0; i<3; i++) ua[i] = R1[((*code)-7)/3 + i*4];
|
||||
for (i=0; i<3; i++) ub[i] = R2[((*code)-7)%3 + i*4];
|
||||
|
||||
lineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
|
||||
for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
|
||||
for (i=0; i<3; i++) pb[i] += ub[i]*beta;
|
||||
|
||||
for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
|
||||
contact[0].depth = *depth;
|
||||
return 1;
|
||||
}
|
||||
|
||||
// okay, we have a face-something intersection (because the separating
|
||||
// axis is perpendicular to a face).
|
||||
|
||||
// @@@ temporary: make deepest vertex on the "other" box the contact point.
|
||||
// @@@ this kind of works, but we need multiple contact points for stability,
|
||||
// @@@ especially for face-face contact.
|
||||
|
||||
dVector3 vertex;
|
||||
if (*code <= 3) {
|
||||
// face from box 1 touches a vertex/edge/face from box 2.
|
||||
dReal sign;
|
||||
for (i=0; i<3; i++) vertex[i] = p2[i];
|
||||
sign = (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0);
|
||||
for (i=0; i<3; i++) vertex[i] += sign * B1 * R2[i*4];
|
||||
sign = (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0);
|
||||
for (i=0; i<3; i++) vertex[i] += sign * B2 * R2[i*4+1];
|
||||
sign = (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0);
|
||||
for (i=0; i<3; i++) vertex[i] += sign * B3 * R2[i*4+2];
|
||||
}
|
||||
else {
|
||||
// face from box 2 touches a vertex/edge/face from box 1.
|
||||
dReal sign;
|
||||
for (i=0; i<3; i++) vertex[i] = p1[i];
|
||||
sign = (dDOT14(normal,R1+0) > 0) ? REAL(1.0) : REAL(-1.0);
|
||||
for (i=0; i<3; i++) vertex[i] += sign * A1 * R1[i*4];
|
||||
sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
|
||||
for (i=0; i<3; i++) vertex[i] += sign * A2 * R1[i*4+1];
|
||||
sign = (dDOT14(normal,R1+2) > 0) ? REAL(1.0) : REAL(-1.0);
|
||||
for (i=0; i<3; i++) vertex[i] += sign * A3 * R1[i*4+2];
|
||||
}
|
||||
for (i=0; i<3; i++) contact[0].pos[i] = vertex[i];
|
||||
contact[0].depth = *depth;
|
||||
return 1;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// general support for geometry objects and classes
|
||||
|
||||
struct dColliderEntry {
|
||||
dColliderFn *fn; // collider function
|
||||
int mode; // 1 = reverse o1 and o2, 2 = no function available
|
||||
};
|
||||
|
||||
static dArray<dxGeomClass*> *classes=0;
|
||||
|
||||
// function pointers and modes for n^2 class collider functions. this is an
|
||||
// n*n matrix stored by row. the functions pointers are extracted from the
|
||||
// class get-collider-function function.
|
||||
static dArray<dColliderEntry> *colliders=0;
|
||||
|
||||
|
||||
static inline void initCollisionArrays()
|
||||
{
|
||||
if (classes==0) {
|
||||
// old way:
|
||||
// classes = (dArray<dxGeomClass*> *) dAllocNoFree (sizeof(dArrayBase));
|
||||
// classes->constructor();
|
||||
classes = new dArray<dxGeomClass*>;
|
||||
classes->setSize (1); // force allocation of array data memory
|
||||
dAllocDontReport (classes);
|
||||
dAllocDontReport (classes->data());
|
||||
classes->setSize (0);
|
||||
}
|
||||
if (colliders==0) {
|
||||
// old way:
|
||||
// colliders=(dArray<dColliderEntry> *)dAllocNoFree (sizeof(dArrayBase));
|
||||
// colliders->constructor();
|
||||
colliders = new dArray<dColliderEntry>;
|
||||
colliders->setSize (1); // force allocation of array data memory
|
||||
dAllocDontReport (colliders);
|
||||
dAllocDontReport (colliders->data());
|
||||
colliders->setSize (0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int dCreateGeomClass (const dGeomClass *c)
|
||||
{
|
||||
dUASSERT(c && c->bytes >= 0 && c->collider && c->aabb,"bad geom class");
|
||||
initCollisionArrays();
|
||||
|
||||
int n = classes->size();
|
||||
dxGeomClass *gc = (dxGeomClass*) dAlloc (sizeof(dxGeomClass));
|
||||
dAllocDontReport (gc);
|
||||
gc->collider = c->collider;
|
||||
gc->aabb = c->aabb;
|
||||
gc->aabb_test = c->aabb_test;
|
||||
gc->dtor = c->dtor;
|
||||
gc->num = n;
|
||||
gc->size = SIZEOF_DXGEOM + c->bytes;
|
||||
classes->push (gc);
|
||||
|
||||
// make room for n^2 class collider function pointers - these entries will
|
||||
// be filled as dCollide() is called.
|
||||
colliders->setSize ((n+1)*(n+1));
|
||||
memset (colliders->data(),0,(n+1)*(n+1)*sizeof(dColliderEntry));
|
||||
|
||||
return n;
|
||||
}
|
||||
|
||||
|
||||
int dCollide (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact,
|
||||
int skip)
|
||||
{
|
||||
int i,c1,c2,a1,a2,count,swap;
|
||||
dColliderFn *fn;
|
||||
dAASSERT(o1 && o2 && contact);
|
||||
dUASSERT(classes && colliders,"no registered geometry classes");
|
||||
|
||||
// no contacts if both geoms on the same body, and the body is not 0
|
||||
if (o1->body == o2->body && o1->body) return 0;
|
||||
|
||||
dColliderEntry *colliders2 = colliders->data();
|
||||
c1 = o1->_class->num;
|
||||
c2 = o2->_class->num;
|
||||
a1 = c1 * classes->size() + c2; // address 1 in collider array
|
||||
a2 = c2 * classes->size() + c1; // address 2 in collider array
|
||||
swap = 0; // set to 1 to swap normals before returning
|
||||
|
||||
// return if there are no collider functions available
|
||||
if ((colliders2[a1].mode==2) || (colliders2[a2].mode==2)) return 0;
|
||||
|
||||
if ((fn = colliders2[a1].fn)) {
|
||||
swap = colliders2[a1].mode;
|
||||
if (swap) count = (*fn) (o2,o1,flags,contact,skip);
|
||||
else count = (*fn) (o1,o2,flags,contact,skip);
|
||||
}
|
||||
else if ((fn = (*classes)[c1]->collider (c2))) {
|
||||
colliders2 [a2].fn = fn;
|
||||
colliders2 [a2].mode = 1;
|
||||
colliders2 [a1].fn = fn; // do mode=0 assignment second so that
|
||||
colliders2 [a1].mode = 0; // diagonal entries will have mode 0
|
||||
count = (*fn) (o1,o2,flags,contact,skip);
|
||||
swap = 0;
|
||||
}
|
||||
else if ((fn = (*classes)[c2]->collider (c1))) {
|
||||
colliders2 [a1].fn = fn;
|
||||
colliders2 [a1].mode = 1;
|
||||
colliders2 [a2].fn = fn; // do mode=0 assignment second so that
|
||||
colliders2 [a2].mode = 0; // diagonal entries will have mode 0
|
||||
count = (*fn) (o2,o1,flags,contact,skip);
|
||||
swap = 1;
|
||||
}
|
||||
else {
|
||||
colliders2[a1].mode = 2;
|
||||
colliders2[a2].mode = 2;
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (swap) {
|
||||
for (i=0; i<count; i++) {
|
||||
dContactGeom *c = CONTACT(contact,skip*i);
|
||||
c->normal[0] = -c->normal[0];
|
||||
c->normal[1] = -c->normal[1];
|
||||
c->normal[2] = -c->normal[2];
|
||||
dxGeom *tmp = c->g1;
|
||||
c->g1 = c->g2;
|
||||
c->g2 = tmp;
|
||||
}
|
||||
}
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
|
||||
int dGeomGetClass (dxGeom *g)
|
||||
{
|
||||
dAASSERT (g);
|
||||
return g->_class->num;
|
||||
}
|
||||
|
||||
|
||||
void dGeomSetData (dxGeom *g, void *data)
|
||||
{
|
||||
dAASSERT (g);
|
||||
g->data = data;
|
||||
}
|
||||
|
||||
|
||||
void *dGeomGetData (dxGeom *g)
|
||||
{
|
||||
dAASSERT (g);
|
||||
return g->data;
|
||||
}
|
||||
|
||||
|
||||
void dGeomSetBody (dxGeom *g, dBodyID b)
|
||||
{
|
||||
dAASSERT (g);
|
||||
if (b) {
|
||||
if (!g->body) dFree (g->pos,sizeof(dxPosR));
|
||||
g->body = b;
|
||||
g->pos = b->pos;
|
||||
g->R = b->R;
|
||||
}
|
||||
else {
|
||||
if (g->body) {
|
||||
dxPosR *pr = (dxPosR*) dAlloc (sizeof(dxPosR));
|
||||
g->pos = pr->pos;
|
||||
g->R = pr->R;
|
||||
memcpy (g->pos,g->body->pos,sizeof(g->pos));
|
||||
memcpy (g->R,g->body->R,sizeof(g->R));
|
||||
g->body = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
dBodyID dGeomGetBody (dxGeom *g)
|
||||
{
|
||||
dAASSERT (g);
|
||||
return g->body;
|
||||
}
|
||||
|
||||
|
||||
void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z)
|
||||
{
|
||||
dAASSERT (g);
|
||||
if (g->body) dBodySetPosition (g->body,x,y,z);
|
||||
else {
|
||||
g->pos[0] = x;
|
||||
g->pos[1] = y;
|
||||
g->pos[2] = z;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dGeomSetRotation (dxGeom *g, const dMatrix3 R)
|
||||
{
|
||||
dAASSERT (g);
|
||||
if (g->body) dBodySetRotation (g->body,R);
|
||||
else memcpy (g->R,R,sizeof(dMatrix3));
|
||||
}
|
||||
|
||||
|
||||
const dReal * dGeomGetPosition (dxGeom *g)
|
||||
{
|
||||
dAASSERT (g);
|
||||
return g->pos;
|
||||
}
|
||||
|
||||
|
||||
const dReal * dGeomGetRotation (dxGeom *g)
|
||||
{
|
||||
dAASSERT (g);
|
||||
return g->R;
|
||||
}
|
||||
|
||||
|
||||
// for external use only. use the CLASSDATA macro inside ODE.
|
||||
|
||||
void * dGeomGetClassData (dxGeom *g)
|
||||
{
|
||||
dAASSERT (g);
|
||||
return (void*) CLASSDATA(g);
|
||||
}
|
||||
|
||||
|
||||
dxGeom * dCreateGeom (int classnum)
|
||||
{
|
||||
dUASSERT (classes && colliders && classnum >= 0 &&
|
||||
classnum < classes->size(),"bad class number");
|
||||
int size = (*classes)[classnum]->size;
|
||||
dxGeom *geom = (dxGeom*) dAlloc (size);
|
||||
memset (geom,0,size); // everything is initially zeroed
|
||||
|
||||
geom->_class = (*classes)[classnum];
|
||||
geom->data = 0;
|
||||
geom->body = 0;
|
||||
|
||||
dxPosR *pr = (dxPosR*) dAlloc (sizeof(dxPosR));
|
||||
geom->pos = pr->pos;
|
||||
geom->R = pr->R;
|
||||
dSetZero (geom->pos,4);
|
||||
dRSetIdentity (geom->R);
|
||||
|
||||
return geom;
|
||||
}
|
||||
|
||||
|
||||
void dGeomDestroy (dxGeom *g)
|
||||
{
|
||||
dAASSERT (g);
|
||||
if (g->spaceid) dSpaceRemove (g->spaceid,g);
|
||||
if (g->_class->dtor) g->_class->dtor (g);
|
||||
if (!g->body) dFree (g->pos,sizeof(dxPosR));
|
||||
dFree (g,g->_class->size);
|
||||
}
|
||||
|
||||
|
||||
void dGeomGetAABB (dxGeom *g, dReal aabb[6])
|
||||
{
|
||||
dAASSERT (g);
|
||||
g->_class->aabb (g,aabb);
|
||||
}
|
||||
|
||||
|
||||
dReal *dGeomGetSpaceAABB (dxGeom *g)
|
||||
{
|
||||
dAASSERT (g);
|
||||
return g->space_aabb;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// data for the standard classes
|
||||
|
||||
struct dxSphere {
|
||||
dReal radius; // sphere radius
|
||||
};
|
||||
|
||||
struct dxBox {
|
||||
dVector3 side; // side lengths (x,y,z)
|
||||
};
|
||||
|
||||
struct dxCCylinder { // capped cylinder
|
||||
dReal radius,lz; // radius, length along z axis */
|
||||
};
|
||||
|
||||
struct dxPlane {
|
||||
dReal p[4];
|
||||
};
|
||||
|
||||
struct dxGeomGroup {
|
||||
dArray<dxGeom*> parts; // all the geoms that make up the group
|
||||
};
|
||||
|
||||
//****************************************************************************
|
||||
// primitive collision functions
|
||||
// same interface as dCollide().
|
||||
// S=sphere, B=box, C=capped cylinder, P=plane, G=group, T=transform
|
||||
|
||||
int dCollideSS (const dxGeom *o1, const dxGeom *o2, int flags,
|
||||
dContactGeom *contact, int skip)
|
||||
{
|
||||
dIASSERT (skip >= (int)sizeof(dContactGeom));
|
||||
dIASSERT (o1->_class->num == dSphereClass);
|
||||
dIASSERT (o2->_class->num == dSphereClass);
|
||||
dxSphere *s1 = (dxSphere*) CLASSDATA(o1);
|
||||
dxSphere *s2 = (dxSphere*) CLASSDATA(o2);
|
||||
contact->g1 = const_cast<dxGeom*> (o1);
|
||||
contact->g2 = const_cast<dxGeom*> (o2);
|
||||
return dCollideSpheres (o1->pos,s1->radius,
|
||||
o2->pos,s2->radius,contact);
|
||||
}
|
||||
|
||||
|
||||
int dCollideSB (const dxGeom *o1, const dxGeom *o2, int flags,
|
||||
dContactGeom *contact, int skip)
|
||||
{
|
||||
// this is easy. get the sphere center `p' relative to the box, and then clip
|
||||
// that to the boundary of the box (call that point `q'). if q is on the
|
||||
// boundary of the box and |p-q| is <= sphere radius, they touch.
|
||||
// if q is inside the box, the sphere is inside the box, so set a contact
|
||||
// normal to push the sphere to the closest box edge.
|
||||
|
||||
dVector3 l,t,p,q,r;
|
||||
dReal depth;
|
||||
int onborder = 0;
|
||||
|
||||
dIASSERT (skip >= (int)sizeof(dContactGeom));
|
||||
dIASSERT (o1->_class->num == dSphereClass);
|
||||
dIASSERT (o2->_class->num == dBoxClass);
|
||||
dxSphere *sphere = (dxSphere*) CLASSDATA(o1);
|
||||
dxBox *box = (dxBox*) CLASSDATA(o2);
|
||||
|
||||
contact->g1 = const_cast<dxGeom*> (o1);
|
||||
contact->g2 = const_cast<dxGeom*> (o2);
|
||||
|
||||
p[0] = o1->pos[0] - o2->pos[0];
|
||||
p[1] = o1->pos[1] - o2->pos[1];
|
||||
p[2] = o1->pos[2] - o2->pos[2];
|
||||
|
||||
l[0] = box->side[0]*REAL(0.5);
|
||||
t[0] = dDOT14(p,o2->R);
|
||||
if (t[0] < -l[0]) { t[0] = -l[0]; onborder = 1; }
|
||||
if (t[0] > l[0]) { t[0] = l[0]; onborder = 1; }
|
||||
|
||||
l[1] = box->side[1]*REAL(0.5);
|
||||
t[1] = dDOT14(p,o2->R+1);
|
||||
if (t[1] < -l[1]) { t[1] = -l[1]; onborder = 1; }
|
||||
if (t[1] > l[1]) { t[1] = l[1]; onborder = 1; }
|
||||
|
||||
t[2] = dDOT14(p,o2->R+2);
|
||||
l[2] = box->side[2]*REAL(0.5);
|
||||
if (t[2] < -l[2]) { t[2] = -l[2]; onborder = 1; }
|
||||
if (t[2] > l[2]) { t[2] = l[2]; onborder = 1; }
|
||||
|
||||
if (!onborder) {
|
||||
// sphere center inside box. find largest `t' value
|
||||
dReal max = dFabs(t[0]);
|
||||
int maxi = 0;
|
||||
for (int i=1; i<3; i++) {
|
||||
dReal tt = dFabs(t[i]);
|
||||
if (tt > max) {
|
||||
max = tt;
|
||||
maxi = i;
|
||||
}
|
||||
}
|
||||
// contact position = sphere center
|
||||
contact->pos[0] = o1->pos[0];
|
||||
contact->pos[1] = o1->pos[1];
|
||||
contact->pos[2] = o1->pos[2];
|
||||
// contact normal aligned with box edge along largest `t' value
|
||||
dVector3 tmp;
|
||||
tmp[0] = 0;
|
||||
tmp[1] = 0;
|
||||
tmp[2] = 0;
|
||||
tmp[maxi] = (t[maxi] > 0) ? REAL(1.0) : REAL(-1.0);
|
||||
dMULTIPLY0_331 (contact->normal,o2->R,tmp);
|
||||
// contact depth = distance to wall along normal plus radius
|
||||
contact->depth = l[maxi] - max + sphere->radius;
|
||||
return 1;
|
||||
}
|
||||
|
||||
t[3] = 0; //@@@ hmmm
|
||||
dMULTIPLY0_331 (q,o2->R,t);
|
||||
r[0] = p[0] - q[0];
|
||||
r[1] = p[1] - q[1];
|
||||
r[2] = p[2] - q[2];
|
||||
depth = sphere->radius - dSqrt(dDOT(r,r));
|
||||
if (depth < 0) return 0;
|
||||
contact->pos[0] = q[0] + o2->pos[0];
|
||||
contact->pos[1] = q[1] + o2->pos[1];
|
||||
contact->pos[2] = q[2] + o2->pos[2];
|
||||
contact->normal[0] = r[0];
|
||||
contact->normal[1] = r[1];
|
||||
contact->normal[2] = r[2];
|
||||
dNormalize3 (contact->normal);
|
||||
contact->depth = depth;
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
int dCollideSP (const dxGeom *o1, const dxGeom *o2, int flags,
|
||||
dContactGeom *contact, int skip)
|
||||
{
|
||||
dIASSERT (skip >= (int)sizeof(dContactGeom));
|
||||
dIASSERT (o1->_class->num == dSphereClass);
|
||||
dIASSERT (o2->_class->num == dPlaneClass);
|
||||
contact->g1 = const_cast<dxGeom*> (o1);
|
||||
contact->g2 = const_cast<dxGeom*> (o2);
|
||||
dxSphere *sphere = (dxSphere*) CLASSDATA(o1);
|
||||
dxPlane *plane = (dxPlane*) CLASSDATA(o2);
|
||||
dReal k = dDOT (o1->pos,plane->p);
|
||||
dReal depth = plane->p[3] - k + sphere->radius;
|
||||
if (depth >= 0) {
|
||||
contact->normal[0] = plane->p[0];
|
||||
contact->normal[1] = plane->p[1];
|
||||
contact->normal[2] = plane->p[2];
|
||||
contact->pos[0] = o1->pos[0] - plane->p[0] * sphere->radius;
|
||||
contact->pos[1] = o1->pos[1] - plane->p[1] * sphere->radius;
|
||||
contact->pos[2] = o1->pos[2] - plane->p[2] * sphere->radius;
|
||||
contact->depth = depth;
|
||||
return 1;
|
||||
}
|
||||
else return 0;
|
||||
}
|
||||
|
||||
|
||||
int dCollideBB (const dxGeom *o1, const dxGeom *o2, int flags,
|
||||
dContactGeom *contact, int skip)
|
||||
{
|
||||
dVector3 normal;
|
||||
dReal depth;
|
||||
int code;
|
||||
dxBox *b1 = (dxBox*) CLASSDATA(o1);
|
||||
dxBox *b2 = (dxBox*) CLASSDATA(o2);
|
||||
int num = dBoxBox (o1->pos,o1->R,b1->side, o2->pos,o2->R,b2->side,
|
||||
normal,&depth,&code,flags & NUMC_MASK,contact,skip);
|
||||
for (int i=0; i<num; i++) {
|
||||
CONTACT(contact,i*skip)->normal[0] = -normal[0];
|
||||
CONTACT(contact,i*skip)->normal[1] = -normal[1];
|
||||
CONTACT(contact,i*skip)->normal[2] = -normal[2];
|
||||
CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
|
||||
CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2);
|
||||
}
|
||||
return num;
|
||||
}
|
||||
|
||||
|
||||
int dCollideBP (const dxGeom *o1, const dxGeom *o2,
|
||||
int flags, dContactGeom *contact, int skip)
|
||||
{
|
||||
dIASSERT (skip >= (int)sizeof(dContactGeom));
|
||||
dIASSERT (o1->_class->num == dBoxClass);
|
||||
dIASSERT (o2->_class->num == dPlaneClass);
|
||||
contact->g1 = const_cast<dxGeom*> (o1);
|
||||
contact->g2 = const_cast<dxGeom*> (o2);
|
||||
dxBox *box = (dxBox*) CLASSDATA(o1);
|
||||
dxPlane *plane = (dxPlane*) CLASSDATA(o2);
|
||||
int ret = 0;
|
||||
|
||||
//@@@ problem: using 4-vector (plane->p) as 3-vector (normal).
|
||||
const dReal *R = o1->R; // rotation of box
|
||||
const dReal *n = plane->p; // normal vector
|
||||
|
||||
// project sides lengths along normal vector, get absolute values
|
||||
dReal Q1 = dDOT14(n,R+0);
|
||||
dReal Q2 = dDOT14(n,R+1);
|
||||
dReal Q3 = dDOT14(n,R+2);
|
||||
dReal A1 = box->side[0] * Q1;
|
||||
dReal A2 = box->side[1] * Q2;
|
||||
dReal A3 = box->side[2] * Q3;
|
||||
dReal B1 = dFabs(A1);
|
||||
dReal B2 = dFabs(A2);
|
||||
dReal B3 = dFabs(A3);
|
||||
|
||||
// early exit test
|
||||
dReal depth = plane->p[3] + REAL(0.5)*(B1+B2+B3) - dDOT(n,o1->pos);
|
||||
if (depth < 0) return 0;
|
||||
|
||||
// find number of contacts requested
|
||||
int maxc = flags & NUMC_MASK;
|
||||
if (maxc < 1) maxc = 1;
|
||||
if (maxc > 3) maxc = 3; // no more than 3 contacts per box allowed
|
||||
|
||||
// find deepest point
|
||||
dVector3 p;
|
||||
p[0] = o1->pos[0];
|
||||
p[1] = o1->pos[1];
|
||||
p[2] = o1->pos[2];
|
||||
#define FOO(i,op) \
|
||||
p[0] op REAL(0.5)*box->side[i] * R[0+i]; \
|
||||
p[1] op REAL(0.5)*box->side[i] * R[4+i]; \
|
||||
p[2] op REAL(0.5)*box->side[i] * R[8+i];
|
||||
#define BAR(i,iinc) if (A ## iinc > 0) { FOO(i,-=) } else { FOO(i,+=) }
|
||||
BAR(0,1);
|
||||
BAR(1,2);
|
||||
BAR(2,3);
|
||||
#undef FOO
|
||||
#undef BAR
|
||||
|
||||
// the deepest point is the first contact point
|
||||
contact->pos[0] = p[0];
|
||||
contact->pos[1] = p[1];
|
||||
contact->pos[2] = p[2];
|
||||
contact->normal[0] = n[0];
|
||||
contact->normal[1] = n[1];
|
||||
contact->normal[2] = n[2];
|
||||
contact->depth = depth;
|
||||
ret = 1; // ret is number of contact points found so far
|
||||
if (maxc == 1) goto done;
|
||||
|
||||
// get the second and third contact points by starting from `p' and going
|
||||
// along the two sides with the smallest projected length.
|
||||
|
||||
#define FOO(i,j,op) \
|
||||
CONTACT(contact,i*skip)->pos[0] = p[0] op box->side[j] * R[0+j]; \
|
||||
CONTACT(contact,i*skip)->pos[1] = p[1] op box->side[j] * R[4+j]; \
|
||||
CONTACT(contact,i*skip)->pos[2] = p[2] op box->side[j] * R[8+j];
|
||||
#define BAR(ctact,side,sideinc) \
|
||||
depth -= B ## sideinc; \
|
||||
if (depth < 0) goto done; \
|
||||
if (A ## sideinc > 0) { FOO(ctact,side,+) } else { FOO(ctact,side,-) } \
|
||||
CONTACT(contact,ctact*skip)->depth = depth; \
|
||||
ret++;
|
||||
|
||||
CONTACT(contact,skip)->normal[0] = n[0];
|
||||
CONTACT(contact,skip)->normal[1] = n[1];
|
||||
CONTACT(contact,skip)->normal[2] = n[2];
|
||||
if (maxc == 3) {
|
||||
CONTACT(contact,2*skip)->normal[0] = n[0];
|
||||
CONTACT(contact,2*skip)->normal[1] = n[1];
|
||||
CONTACT(contact,2*skip)->normal[2] = n[2];
|
||||
}
|
||||
|
||||
if (B1 < B2) {
|
||||
if (B3 < B1) goto use_side_3; else {
|
||||
BAR(1,0,1); // use side 1
|
||||
if (maxc == 2) goto done;
|
||||
if (B2 < B3) goto contact2_2; else goto contact2_3;
|
||||
}
|
||||
}
|
||||
else {
|
||||
if (B3 < B2) {
|
||||
use_side_3: // use side 3
|
||||
BAR(1,2,3);
|
||||
if (maxc == 2) goto done;
|
||||
if (B1 < B2) goto contact2_1; else goto contact2_2;
|
||||
}
|
||||
else {
|
||||
BAR(1,1,2); // use side 2
|
||||
if (maxc == 2) goto done;
|
||||
if (B1 < B3) goto contact2_1; else goto contact2_3;
|
||||
}
|
||||
}
|
||||
|
||||
contact2_1: BAR(2,0,1); goto done;
|
||||
contact2_2: BAR(2,1,2); goto done;
|
||||
contact2_3: BAR(2,2,3); goto done;
|
||||
#undef FOO
|
||||
#undef BAR
|
||||
|
||||
done:
|
||||
for (int i=0; i<ret; i++) {
|
||||
CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
|
||||
CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
int dCollideCS (const dxGeom *o1, const dxGeom *o2, int flags,
|
||||
dContactGeom *contact, int skip)
|
||||
{
|
||||
dIASSERT (skip >= (int)sizeof(dContactGeom));
|
||||
dIASSERT (o1->_class->num == dCCylinderClass);
|
||||
dIASSERT (o2->_class->num == dSphereClass);
|
||||
contact->g1 = const_cast<dxGeom*> (o1);
|
||||
contact->g2 = const_cast<dxGeom*> (o2);
|
||||
dxCCylinder *ccyl = (dxCCylinder*) CLASSDATA(o1);
|
||||
dxSphere *sphere = (dxSphere*) CLASSDATA(o2);
|
||||
|
||||
// find the point on the cylinder axis that is closest to the sphere
|
||||
dReal alpha =
|
||||
o1->R[2] * (o2->pos[0] - o1->pos[0]) +
|
||||
o1->R[6] * (o2->pos[1] - o1->pos[1]) +
|
||||
o1->R[10] * (o2->pos[2] - o1->pos[2]);
|
||||
dReal lz2 = ccyl->lz * REAL(0.5);
|
||||
if (alpha > lz2) alpha = lz2;
|
||||
if (alpha < -lz2) alpha = -lz2;
|
||||
|
||||
// collide the spheres
|
||||
dVector3 p;
|
||||
p[0] = o1->pos[0] + alpha * o1->R[2];
|
||||
p[1] = o1->pos[1] + alpha * o1->R[6];
|
||||
p[2] = o1->pos[2] + alpha * o1->R[10];
|
||||
return dCollideSpheres (p,ccyl->radius,o2->pos,sphere->radius,contact);
|
||||
}
|
||||
|
||||
|
||||
int dCollideCB (const dxGeom *o1, const dxGeom *o2, int flags,
|
||||
dContactGeom *contact, int skip)
|
||||
{
|
||||
dIASSERT (skip >= (int)sizeof(dContactGeom));
|
||||
dIASSERT (o1->_class->num == dCCylinderClass);
|
||||
dIASSERT (o2->_class->num == dBoxClass);
|
||||
contact->g1 = const_cast<dxGeom*> (o1);
|
||||
contact->g2 = const_cast<dxGeom*> (o2);
|
||||
dxCCylinder *cyl = (dxCCylinder*) CLASSDATA(o1);
|
||||
dxBox *box = (dxBox*) CLASSDATA(o2);
|
||||
|
||||
// get p1,p2 = cylinder axis endpoints, get radius
|
||||
dVector3 p1,p2;
|
||||
dReal clen = cyl->lz * REAL(0.5);
|
||||
p1[0] = o1->pos[0] + clen * o1->R[2];
|
||||
p1[1] = o1->pos[1] + clen * o1->R[6];
|
||||
p1[2] = o1->pos[2] + clen * o1->R[10];
|
||||
p2[0] = o1->pos[0] - clen * o1->R[2];
|
||||
p2[1] = o1->pos[1] - clen * o1->R[6];
|
||||
p2[2] = o1->pos[2] - clen * o1->R[10];
|
||||
dReal radius = cyl->radius;
|
||||
|
||||
// copy out box center, rotation matrix, and side array
|
||||
dReal *c = o2->pos;
|
||||
dReal *R = o2->R;
|
||||
dReal *side = box->side;
|
||||
|
||||
// get the closest point between the cylinder axis and the box
|
||||
dVector3 pl,pb;
|
||||
dClosestLineBoxPoints (p1,p2,c,R,side,pl,pb);
|
||||
|
||||
// generate contact point
|
||||
return dCollideSpheres (pl,radius,pb,0,contact);
|
||||
}
|
||||
|
||||
|
||||
// this returns at most one contact point when the two cylinder's axes are not
|
||||
// aligned, and at most two (for stability) when they are aligned.
|
||||
// the algorithm minimizes the distance between two "sample spheres" that are
|
||||
// positioned along the cylinder axes according to:
|
||||
// sphere1 = pos1 + alpha1 * axis1
|
||||
// sphere2 = pos2 + alpha2 * axis2
|
||||
// alpha1 and alpha2 are limited to +/- half the length of the cylinders.
|
||||
// the algorithm works by finding a solution that has both alphas free, or
|
||||
// a solution that has one or both alphas fixed to the ends of the cylinder.
|
||||
|
||||
int dCollideCC (const dxGeom *o1, const dxGeom *o2,
|
||||
int flags, dContactGeom *contact, int skip)
|
||||
{
|
||||
int i;
|
||||
const dReal tolerance = REAL(1e-5);
|
||||
|
||||
dIASSERT (skip >= (int)sizeof(dContactGeom));
|
||||
dIASSERT (o1->_class->num == dCCylinderClass);
|
||||
dIASSERT (o2->_class->num == dCCylinderClass);
|
||||
contact->g1 = const_cast<dxGeom*> (o1);
|
||||
contact->g2 = const_cast<dxGeom*> (o2);
|
||||
dxCCylinder *cyl1 = (dxCCylinder*) CLASSDATA(o1);
|
||||
dxCCylinder *cyl2 = (dxCCylinder*) CLASSDATA(o2);
|
||||
|
||||
// copy out some variables, for convenience
|
||||
dReal lz1 = cyl1->lz * REAL(0.5);
|
||||
dReal lz2 = cyl2->lz * REAL(0.5);
|
||||
dReal *pos1 = o1->pos;
|
||||
dReal *pos2 = o2->pos;
|
||||
dReal axis1[3],axis2[3];
|
||||
axis1[0] = o1->R[2];
|
||||
axis1[1] = o1->R[6];
|
||||
axis1[2] = o1->R[10];
|
||||
axis2[0] = o2->R[2];
|
||||
axis2[1] = o2->R[6];
|
||||
axis2[2] = o2->R[10];
|
||||
|
||||
dReal alpha1,alpha2,sphere1[3],sphere2[3];
|
||||
int fix1 = 0; // 0 if alpha1 is free, +/-1 to fix at +/- lz1
|
||||
int fix2 = 0; // 0 if alpha2 is free, +/-1 to fix at +/- lz2
|
||||
|
||||
for (int count=0; count<9; count++) {
|
||||
// find a trial solution by fixing or not fixing the alphas
|
||||
if (fix1) {
|
||||
if (fix2) {
|
||||
// alpha1 and alpha2 are fixed, so the solution is easy
|
||||
if (fix1 > 0) alpha1 = lz1; else alpha1 = -lz1;
|
||||
if (fix2 > 0) alpha2 = lz2; else alpha2 = -lz2;
|
||||
for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
|
||||
for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
|
||||
}
|
||||
else {
|
||||
// fix alpha1 but let alpha2 be free
|
||||
if (fix1 > 0) alpha1 = lz1; else alpha1 = -lz1;
|
||||
for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
|
||||
alpha2 = (axis2[0]*(sphere1[0]-pos2[0]) +
|
||||
axis2[1]*(sphere1[1]-pos2[1]) +
|
||||
axis2[2]*(sphere1[2]-pos2[2]));
|
||||
for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
|
||||
}
|
||||
}
|
||||
else {
|
||||
if (fix2) {
|
||||
// fix alpha2 but let alpha1 be free
|
||||
if (fix2 > 0) alpha2 = lz2; else alpha2 = -lz2;
|
||||
for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
|
||||
alpha1 = (axis1[0]*(sphere2[0]-pos1[0]) +
|
||||
axis1[1]*(sphere2[1]-pos1[1]) +
|
||||
axis1[2]*(sphere2[2]-pos1[2]));
|
||||
for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
|
||||
}
|
||||
else {
|
||||
// let alpha1 and alpha2 be free
|
||||
// compute determinant of d(d^2)\d(alpha) jacobian
|
||||
dReal a1a2 = dDOT (axis1,axis2);
|
||||
dReal det = REAL(1.0)-a1a2*a1a2;
|
||||
if (det < tolerance) {
|
||||
// the cylinder axes (almost) parallel, so we will generate up to two
|
||||
// contacts. the solution matrix is rank deficient so alpha1 and
|
||||
// alpha2 are related by:
|
||||
// alpha2 = alpha1 + (pos1-pos2)'*axis1 (if axis1==axis2)
|
||||
// or alpha2 = -(alpha1 + (pos1-pos2)'*axis1) (if axis1==-axis2)
|
||||
// first compute where the two cylinders overlap in alpha1 space:
|
||||
if (a1a2 < 0) {
|
||||
axis2[0] = -axis2[0];
|
||||
axis2[1] = -axis2[1];
|
||||
axis2[2] = -axis2[2];
|
||||
}
|
||||
dReal q[3];
|
||||
for (i=0; i<3; i++) q[i] = pos1[i]-pos2[i];
|
||||
dReal k = dDOT (axis1,q);
|
||||
dReal a1lo = -lz1;
|
||||
dReal a1hi = lz1;
|
||||
dReal a2lo = -lz2 - k;
|
||||
dReal a2hi = lz2 - k;
|
||||
dReal lo = (a1lo > a2lo) ? a1lo : a2lo;
|
||||
dReal hi = (a1hi < a2hi) ? a1hi : a2hi;
|
||||
if (lo <= hi) {
|
||||
int num_contacts = flags & NUMC_MASK;
|
||||
if (num_contacts >= 2 && lo < hi) {
|
||||
// generate up to two contacts. if one of those contacts is
|
||||
// not made, fall back on the one-contact strategy.
|
||||
for (i=0; i<3; i++) sphere1[i] = pos1[i] + lo*axis1[i];
|
||||
for (i=0; i<3; i++) sphere2[i] = pos2[i] + (lo+k)*axis2[i];
|
||||
int n1 = dCollideSpheres (sphere1,cyl1->radius,
|
||||
sphere2,cyl2->radius,contact);
|
||||
if (n1) {
|
||||
for (i=0; i<3; i++) sphere1[i] = pos1[i] + hi*axis1[i];
|
||||
for (i=0; i<3; i++) sphere2[i] = pos2[i] + (hi+k)*axis2[i];
|
||||
dContactGeom *c2 = CONTACT(contact,skip);
|
||||
int n2 = dCollideSpheres (sphere1,cyl1->radius,
|
||||
sphere2,cyl2->radius, c2);
|
||||
if (n2) {
|
||||
c2->g1 = const_cast<dxGeom*> (o1);
|
||||
c2->g2 = const_cast<dxGeom*> (o2);
|
||||
return 2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// just one contact to generate, so put it in the middle of
|
||||
// the range
|
||||
alpha1 = (lo + hi) * REAL(0.5);
|
||||
alpha2 = alpha1 + k;
|
||||
for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
|
||||
for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
|
||||
return dCollideSpheres (sphere1,cyl1->radius,
|
||||
sphere2,cyl2->radius,contact);
|
||||
}
|
||||
else return 0;
|
||||
}
|
||||
det = REAL(1.0)/det;
|
||||
dReal delta[3];
|
||||
for (i=0; i<3; i++) delta[i] = pos1[i] - pos2[i];
|
||||
dReal q1 = dDOT (delta,axis1);
|
||||
dReal q2 = dDOT (delta,axis2);
|
||||
alpha1 = det*(a1a2*q2-q1);
|
||||
alpha2 = det*(q2-a1a2*q1);
|
||||
for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
|
||||
for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
|
||||
}
|
||||
}
|
||||
|
||||
// if the alphas are outside their allowed ranges then fix them and
|
||||
// try again
|
||||
if (fix1==0) {
|
||||
if (alpha1 < -lz1) {
|
||||
fix1 = -1;
|
||||
continue;
|
||||
}
|
||||
if (alpha1 > lz1) {
|
||||
fix1 = 1;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
if (fix2==0) {
|
||||
if (alpha2 < -lz2) {
|
||||
fix2 = -1;
|
||||
continue;
|
||||
}
|
||||
if (alpha2 > lz2) {
|
||||
fix2 = 1;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
// unfix the alpha variables if the local distance gradient indicates
|
||||
// that we are not yet at the minimum
|
||||
dReal tmp[3];
|
||||
for (i=0; i<3; i++) tmp[i] = sphere1[i] - sphere2[i];
|
||||
if (fix1) {
|
||||
dReal gradient = dDOT (tmp,axis1);
|
||||
if ((fix1 > 0 && gradient > 0) || (fix1 < 0 && gradient < 0)) {
|
||||
fix1 = 0;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
if (fix2) {
|
||||
dReal gradient = -dDOT (tmp,axis2);
|
||||
if ((fix2 > 0 && gradient > 0) || (fix2 < 0 && gradient < 0)) {
|
||||
fix2 = 0;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
return dCollideSpheres (sphere1,cyl1->radius,sphere2,cyl2->radius,contact);
|
||||
}
|
||||
// if we go through the loop too much, then give up. we should NEVER get to
|
||||
// this point (i hope).
|
||||
dMessage (0,"dCollideCC(): too many iterations");
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int dCollideCP (const dxGeom *o1, const dxGeom *o2, int flags,
|
||||
dContactGeom *contact, int skip)
|
||||
{
|
||||
dIASSERT (skip >= (int)sizeof(dContactGeom));
|
||||
dIASSERT (o1->_class->num == dCCylinderClass);
|
||||
dIASSERT (o2->_class->num == dPlaneClass);
|
||||
dxCCylinder *ccyl = (dxCCylinder*) CLASSDATA(o1);
|
||||
dxPlane *plane = (dxPlane*) CLASSDATA(o2);
|
||||
|
||||
// collide the deepest capping sphere with the plane
|
||||
dReal sign = (dDOT14 (plane->p,o1->R+2) > 0) ? REAL(-1.0) : REAL(1.0);
|
||||
dVector3 p;
|
||||
p[0] = o1->pos[0] + o1->R[2] * ccyl->lz * REAL(0.5) * sign;
|
||||
p[1] = o1->pos[1] + o1->R[6] * ccyl->lz * REAL(0.5) * sign;
|
||||
p[2] = o1->pos[2] + o1->R[10] * ccyl->lz * REAL(0.5) * sign;
|
||||
|
||||
dReal k = dDOT (p,plane->p);
|
||||
dReal depth = plane->p[3] - k + ccyl->radius;
|
||||
if (depth < 0) return 0;
|
||||
contact->normal[0] = plane->p[0];
|
||||
contact->normal[1] = plane->p[1];
|
||||
contact->normal[2] = plane->p[2];
|
||||
contact->pos[0] = p[0] - plane->p[0] * ccyl->radius;
|
||||
contact->pos[1] = p[1] - plane->p[1] * ccyl->radius;
|
||||
contact->pos[2] = p[2] - plane->p[2] * ccyl->radius;
|
||||
contact->depth = depth;
|
||||
|
||||
int ncontacts = 1;
|
||||
if ((flags & NUMC_MASK) >= 2) {
|
||||
// collide the other capping sphere with the plane
|
||||
p[0] = o1->pos[0] - o1->R[2] * ccyl->lz * REAL(0.5) * sign;
|
||||
p[1] = o1->pos[1] - o1->R[6] * ccyl->lz * REAL(0.5) * sign;
|
||||
p[2] = o1->pos[2] - o1->R[10] * ccyl->lz * REAL(0.5) * sign;
|
||||
|
||||
k = dDOT (p,plane->p);
|
||||
depth = plane->p[3] - k + ccyl->radius;
|
||||
if (depth >= 0) {
|
||||
dContactGeom *c2 = CONTACT(contact,skip);
|
||||
c2->normal[0] = plane->p[0];
|
||||
c2->normal[1] = plane->p[1];
|
||||
c2->normal[2] = plane->p[2];
|
||||
c2->pos[0] = p[0] - plane->p[0] * ccyl->radius;
|
||||
c2->pos[1] = p[1] - plane->p[1] * ccyl->radius;
|
||||
c2->pos[2] = p[2] - plane->p[2] * ccyl->radius;
|
||||
c2->depth = depth;
|
||||
ncontacts = 2;
|
||||
}
|
||||
}
|
||||
|
||||
for (int i=0; i < ncontacts; i++) {
|
||||
CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
|
||||
CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2);
|
||||
}
|
||||
return ncontacts;
|
||||
}
|
||||
|
||||
|
||||
// this collides a group with another geom. the other geom can also be a
|
||||
// group, but this case is not handled specially.
|
||||
|
||||
int dCollideG (const dxGeom *o1, const dxGeom *o2, int flags,
|
||||
dContactGeom *contact, int skip)
|
||||
{
|
||||
dxGeomGroup *gr = (dxGeomGroup*) CLASSDATA(o1);
|
||||
int numleft = flags & NUMC_MASK;
|
||||
if (numleft == 0) numleft = 1;
|
||||
flags &= ~NUMC_MASK;
|
||||
int num=0,i=0;
|
||||
while (i < gr->parts.size() && numleft > 0) {
|
||||
int n = dCollide (gr->parts[i],const_cast<dxGeom*>(o2),
|
||||
flags | numleft,contact,skip);
|
||||
contact = CONTACT (contact,skip*n);
|
||||
numleft -= n;
|
||||
num += n;
|
||||
i++;
|
||||
}
|
||||
return num;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// standard classes
|
||||
|
||||
SHAREDLIBEXPORT int dSphereClass = -1;
|
||||
SHAREDLIBEXPORT int dBoxClass = -1;
|
||||
SHAREDLIBEXPORT int dCCylinderClass = -1;
|
||||
SHAREDLIBEXPORT int dPlaneClass = -1;
|
||||
|
||||
|
||||
static dColliderFn * dSphereColliderFn (int num)
|
||||
{
|
||||
if (num == dSphereClass) return (dColliderFn *) &dCollideSS;
|
||||
if (num == dBoxClass) return (dColliderFn *) &dCollideSB;
|
||||
if (num == dPlaneClass) return (dColliderFn *) &dCollideSP;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
static void dSphereAABB (dxGeom *geom, dReal aabb[6])
|
||||
{
|
||||
dxSphere *s = (dxSphere*) CLASSDATA(geom);
|
||||
aabb[0] = geom->pos[0] - s->radius;
|
||||
aabb[1] = geom->pos[0] + s->radius;
|
||||
aabb[2] = geom->pos[1] - s->radius;
|
||||
aabb[3] = geom->pos[1] + s->radius;
|
||||
aabb[4] = geom->pos[2] - s->radius;
|
||||
aabb[5] = geom->pos[2] + s->radius;
|
||||
}
|
||||
|
||||
|
||||
static dColliderFn * dBoxColliderFn (int num)
|
||||
{
|
||||
if (num == dBoxClass) return (dColliderFn *) &dCollideBB;
|
||||
if (num == dPlaneClass) return (dColliderFn *) &dCollideBP;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
static void dBoxAABB (dxGeom *geom, dReal aabb[6])
|
||||
{
|
||||
dxBox *b = (dxBox*) CLASSDATA(geom);
|
||||
dReal xrange = REAL(0.5) * (dFabs (geom->R[0] * b->side[0]) +
|
||||
dFabs (geom->R[1] * b->side[1]) + dFabs (geom->R[2] * b->side[2]));
|
||||
dReal yrange = REAL(0.5) * (dFabs (geom->R[4] * b->side[0]) +
|
||||
dFabs (geom->R[5] * b->side[1]) + dFabs (geom->R[6] * b->side[2]));
|
||||
dReal zrange = REAL(0.5) * (dFabs (geom->R[8] * b->side[0]) +
|
||||
dFabs (geom->R[9] * b->side[1]) + dFabs (geom->R[10] * b->side[2]));
|
||||
aabb[0] = geom->pos[0] - xrange;
|
||||
aabb[1] = geom->pos[0] + xrange;
|
||||
aabb[2] = geom->pos[1] - yrange;
|
||||
aabb[3] = geom->pos[1] + yrange;
|
||||
aabb[4] = geom->pos[2] - zrange;
|
||||
aabb[5] = geom->pos[2] + zrange;
|
||||
}
|
||||
|
||||
|
||||
static dColliderFn * dCCylinderColliderFn (int num)
|
||||
{
|
||||
if (num == dSphereClass) return (dColliderFn *) &dCollideCS;
|
||||
if (num == dPlaneClass) return (dColliderFn *) &dCollideCP;
|
||||
if (num == dCCylinderClass) return (dColliderFn *) &dCollideCC;
|
||||
if (num == dBoxClass) return (dColliderFn *) &dCollideCB;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
static void dCCylinderAABB (dxGeom *geom, dReal aabb[6])
|
||||
{
|
||||
dxCCylinder *c = (dxCCylinder*) CLASSDATA(geom);
|
||||
dReal xrange = dFabs(geom->R[2] * c->lz) * REAL(0.5) + c->radius;
|
||||
dReal yrange = dFabs(geom->R[6] * c->lz) * REAL(0.5) + c->radius;
|
||||
dReal zrange = dFabs(geom->R[10] * c->lz) * REAL(0.5) + c->radius;
|
||||
aabb[0] = geom->pos[0] - xrange;
|
||||
aabb[1] = geom->pos[0] + xrange;
|
||||
aabb[2] = geom->pos[1] - yrange;
|
||||
aabb[3] = geom->pos[1] + yrange;
|
||||
aabb[4] = geom->pos[2] - zrange;
|
||||
aabb[5] = geom->pos[2] + zrange;
|
||||
}
|
||||
|
||||
|
||||
dColliderFn * dPlaneColliderFn (int num)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
static void dPlaneAABB (dxGeom *geom, dReal aabb[6])
|
||||
{
|
||||
// @@@ planes that have normal vectors aligned along an axis can use a
|
||||
// @@@ less comprehensive bounding box.
|
||||
aabb[0] = -dInfinity;
|
||||
aabb[1] = dInfinity;
|
||||
aabb[2] = -dInfinity;
|
||||
aabb[3] = dInfinity;
|
||||
aabb[4] = -dInfinity;
|
||||
aabb[5] = dInfinity;
|
||||
}
|
||||
|
||||
|
||||
dxGeom *dCreateSphere (dSpaceID space, dReal radius)
|
||||
{
|
||||
dAASSERT (radius > 0);
|
||||
if (dSphereClass == -1) {
|
||||
dGeomClass c;
|
||||
c.bytes = sizeof (dxSphere);
|
||||
c.collider = &dSphereColliderFn;
|
||||
c.aabb = &dSphereAABB;
|
||||
c.aabb_test = 0;
|
||||
c.dtor = 0;
|
||||
dSphereClass = dCreateGeomClass (&c);
|
||||
}
|
||||
|
||||
dxGeom *g = dCreateGeom (dSphereClass);
|
||||
if (space) dSpaceAdd (space,g);
|
||||
dxSphere *s = (dxSphere*) CLASSDATA(g);
|
||||
s->radius = radius;
|
||||
return g;
|
||||
}
|
||||
|
||||
|
||||
dxGeom *dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz)
|
||||
{
|
||||
dAASSERT (lx > 0 && ly > 0 && lz > 0);
|
||||
if (dBoxClass == -1) {
|
||||
dGeomClass c;
|
||||
c.bytes = sizeof (dxBox);
|
||||
c.collider = &dBoxColliderFn;
|
||||
c.aabb = &dBoxAABB;
|
||||
c.aabb_test = 0;
|
||||
c.dtor = 0;
|
||||
dBoxClass = dCreateGeomClass (&c);
|
||||
}
|
||||
|
||||
dxGeom *g = dCreateGeom (dBoxClass);
|
||||
if (space) dSpaceAdd (space,g);
|
||||
dxBox *b = (dxBox*) CLASSDATA(g);
|
||||
b->side[0] = lx;
|
||||
b->side[1] = ly;
|
||||
b->side[2] = lz;
|
||||
return g;
|
||||
}
|
||||
|
||||
|
||||
dxGeom * dCreateCCylinder (dSpaceID space, dReal radius, dReal length)
|
||||
{
|
||||
dAASSERT (radius > 0 && length > 0);
|
||||
if (dCCylinderClass == -1) {
|
||||
dGeomClass c;
|
||||
c.bytes = sizeof (dxCCylinder);
|
||||
c.collider = &dCCylinderColliderFn;
|
||||
c.aabb = &dCCylinderAABB;
|
||||
c.aabb_test = 0;
|
||||
c.dtor = 0;
|
||||
dCCylinderClass = dCreateGeomClass (&c);
|
||||
}
|
||||
|
||||
dxGeom *g = dCreateGeom (dCCylinderClass);
|
||||
if (space) dSpaceAdd (space,g);
|
||||
dxCCylinder *c = (dxCCylinder*) CLASSDATA(g);
|
||||
c->radius = radius;
|
||||
c->lz = length;
|
||||
return g;
|
||||
}
|
||||
|
||||
|
||||
dxGeom *dCreatePlane (dSpaceID space,
|
||||
dReal a, dReal b, dReal c, dReal d)
|
||||
{
|
||||
if (dPlaneClass == -1) {
|
||||
dGeomClass c;
|
||||
c.bytes = sizeof (dxPlane);
|
||||
c.collider = &dPlaneColliderFn;
|
||||
c.aabb = &dPlaneAABB;
|
||||
c.aabb_test = 0;
|
||||
c.dtor = 0;
|
||||
dPlaneClass = dCreateGeomClass (&c);
|
||||
}
|
||||
|
||||
dxGeom *g = dCreateGeom (dPlaneClass);
|
||||
if (space) dSpaceAdd (space,g);
|
||||
dxPlane *p = (dxPlane*) CLASSDATA(g);
|
||||
|
||||
// make sure plane normal has unit length
|
||||
dReal l = a*a + b*b + c*c;
|
||||
if (l > 0) {
|
||||
l = dRecipSqrt(l);
|
||||
p->p[0] = a*l;
|
||||
p->p[1] = b*l;
|
||||
p->p[2] = c*l;
|
||||
p->p[3] = d*l;
|
||||
}
|
||||
else {
|
||||
p->p[0] = 1;
|
||||
p->p[1] = 0;
|
||||
p->p[2] = 0;
|
||||
p->p[3] = 0;
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
|
||||
void dGeomSphereSetRadius (dGeomID g, dReal radius)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dSphereClass,"argument not a sphere");
|
||||
dAASSERT (radius > 0);
|
||||
dxSphere *s = (dxSphere*) CLASSDATA(g);
|
||||
s->radius = radius;
|
||||
}
|
||||
|
||||
|
||||
void dGeomBoxSetLengths (dGeomID g, dReal lx, dReal ly, dReal lz)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dBoxClass,"argument not a box");
|
||||
dAASSERT (lx > 0 && ly > 0 && lz > 0);
|
||||
dxBox *b = (dxBox*) CLASSDATA(g);
|
||||
b->side[0] = lx;
|
||||
b->side[1] = ly;
|
||||
b->side[2] = lz;
|
||||
}
|
||||
|
||||
|
||||
void dGeomPlaneSetParams (dGeomID g, dReal a, dReal b, dReal c, dReal d)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dPlaneClass,"argument not a plane");
|
||||
dxPlane *p = (dxPlane*) CLASSDATA(g);
|
||||
p->p[0] = a;
|
||||
p->p[1] = b;
|
||||
p->p[2] = c;
|
||||
p->p[3] = d;
|
||||
}
|
||||
|
||||
|
||||
void dGeomCCylinderSetParams (dGeomID g, dReal radius, dReal length)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dCCylinderClass,"argument not a ccylinder");
|
||||
dAASSERT (radius > 0 && length > 0);
|
||||
dxCCylinder *c = (dxCCylinder*) CLASSDATA(g);
|
||||
c->radius = radius;
|
||||
c->lz = length;
|
||||
}
|
||||
|
||||
|
||||
dReal dGeomSphereGetRadius (dGeomID g)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dSphereClass,"argument not a sphere");
|
||||
dxSphere *s = (dxSphere*) CLASSDATA(g);
|
||||
return s->radius;
|
||||
}
|
||||
|
||||
|
||||
void dGeomBoxGetLengths (dGeomID g, dVector3 result)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dBoxClass,"argument not a box");
|
||||
dxBox *b = (dxBox*) CLASSDATA(g);
|
||||
result[0] = b->side[0];
|
||||
result[1] = b->side[1];
|
||||
result[2] = b->side[2];
|
||||
}
|
||||
|
||||
|
||||
void dGeomPlaneGetParams (dGeomID g, dVector4 result)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dPlaneClass,"argument not a plane");
|
||||
dxPlane *p = (dxPlane*) CLASSDATA(g);
|
||||
result[0] = p->p[0];
|
||||
result[1] = p->p[1];
|
||||
result[2] = p->p[2];
|
||||
result[3] = p->p[3];
|
||||
}
|
||||
|
||||
|
||||
void dGeomCCylinderGetParams (dGeomID g, dReal *radius, dReal *length)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dCCylinderClass,"argument not a ccylinder");
|
||||
dxCCylinder *c = (dxCCylinder*) CLASSDATA(g);
|
||||
*radius = c->radius;
|
||||
*length = c->lz;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// geom group
|
||||
|
||||
int dGeomGroupClass = -1;
|
||||
|
||||
static dColliderFn * dGeomGroupColliderFn (int num)
|
||||
{
|
||||
return (dColliderFn *) &dCollideG;
|
||||
}
|
||||
|
||||
|
||||
static void dGeomGroupAABB (dxGeom *geom, dReal aabb[6])
|
||||
{
|
||||
dxGeomGroup *gr = (dxGeomGroup*) CLASSDATA(geom);
|
||||
aabb[0] = dInfinity;
|
||||
aabb[1] = -dInfinity;
|
||||
aabb[2] = dInfinity;
|
||||
aabb[3] = -dInfinity;
|
||||
aabb[4] = dInfinity;
|
||||
aabb[5] = -dInfinity;
|
||||
int i,j;
|
||||
for (i=0; i < gr->parts.size(); i++) {
|
||||
dReal aabb2[6];
|
||||
gr->parts[i]->_class->aabb (gr->parts[i],aabb2);
|
||||
for (j=0; j<6; j += 2) if (aabb2[j] < aabb[j]) aabb[j] = aabb2[j];
|
||||
for (j=1; j<6; j += 2) if (aabb2[j] > aabb[j]) aabb[j] = aabb2[j];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void dGeomGroupDtor (dxGeom *geom)
|
||||
{
|
||||
dxGeomGroup *gr = (dxGeomGroup*) CLASSDATA(geom);
|
||||
gr->parts.~dArray();
|
||||
}
|
||||
|
||||
|
||||
dxGeom *dCreateGeomGroup (dSpaceID space)
|
||||
{
|
||||
if (dGeomGroupClass == -1) {
|
||||
dGeomClass c;
|
||||
c.bytes = sizeof (dxGeomGroup);
|
||||
c.collider = &dGeomGroupColliderFn;
|
||||
c.aabb = &dGeomGroupAABB;
|
||||
c.aabb_test = 0;
|
||||
c.dtor = &dGeomGroupDtor;
|
||||
dGeomGroupClass = dCreateGeomClass (&c);
|
||||
}
|
||||
|
||||
dxGeom *g = dCreateGeom (dGeomGroupClass);
|
||||
if (space) dSpaceAdd (space,g);
|
||||
dxGeomGroup *gr = (dxGeomGroup*) CLASSDATA(g);
|
||||
gr->parts.constructor();
|
||||
return g;
|
||||
}
|
||||
|
||||
|
||||
void dGeomGroupAdd (dxGeom *g, dxGeom *x)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dGeomGroupClass,"argument not a geomgroup");
|
||||
dxGeomGroup *gr = (dxGeomGroup*) CLASSDATA(g);
|
||||
gr->parts.push (x);
|
||||
}
|
||||
|
||||
|
||||
void dGeomGroupRemove (dxGeom *g, dxGeom *x)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dGeomGroupClass,"argument not a geomgroup");
|
||||
dxGeomGroup *gr = (dxGeomGroup*) CLASSDATA(g);
|
||||
for (int i=0; i < gr->parts.size(); i++) {
|
||||
if (gr->parts[i] == x) {
|
||||
gr->parts.remove (i);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int dGeomGroupGetNumGeoms (dxGeom *g)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dGeomGroupClass,"argument not a geomgroup");
|
||||
dxGeomGroup *gr = (dxGeomGroup*) CLASSDATA(g);
|
||||
return gr->parts.size();
|
||||
}
|
||||
|
||||
|
||||
dxGeom * dGeomGroupGetGeom (dxGeom *g, int i)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dGeomGroupClass,"argument not a geomgroup");
|
||||
dxGeomGroup *gr = (dxGeomGroup*) CLASSDATA(g);
|
||||
dAASSERT (i >= 0 && i < gr->parts.size());
|
||||
return gr->parts[i];
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// transformed geom
|
||||
|
||||
int dGeomTransformClass = -1;
|
||||
|
||||
struct dxGeomTransform {
|
||||
dxGeom *obj; // object that is being transformed
|
||||
int cleanup; // 1 to destroy obj when destroyed
|
||||
int infomode; // 1 to put Tx geom in dContactGeom g1
|
||||
dVector3 final_pos; // final tx (body tx + relative tx) of the object.
|
||||
dMatrix3 final_R; // this is only set if the AABB function is called
|
||||
}; // by space collision before the collide fn is called
|
||||
|
||||
|
||||
// compute final pos and R for the encapsulated geom object
|
||||
|
||||
static void compute_final_tx (const dxGeom *g)
|
||||
{
|
||||
dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(g);
|
||||
dMULTIPLY0_331 (tr->final_pos,g->R,tr->obj->pos);
|
||||
tr->final_pos[0] += g->pos[0];
|
||||
tr->final_pos[1] += g->pos[1];
|
||||
tr->final_pos[2] += g->pos[2];
|
||||
dMULTIPLY0_333 (tr->final_R,g->R,tr->obj->R);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// this collides a transformed geom with another geom. the other geom can
|
||||
// also be a transformed geom, but this case is not handled specially.
|
||||
|
||||
int dCollideT (const dxGeom *o1, const dxGeom *o2, int flags,
|
||||
dContactGeom *contact, int skip)
|
||||
{
|
||||
dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(o1);
|
||||
if (!tr->obj) return 0;
|
||||
dUASSERT (tr->obj->spaceid==0,
|
||||
"GeomTransform encapsulated object must not be in a space");
|
||||
dUASSERT (tr->obj->body==0,
|
||||
"GeomTransform encapsulated object must not be attach to a body");
|
||||
|
||||
// backup the relative pos and R pointers of the encapsulated geom object,
|
||||
// and the body pointer
|
||||
dReal *posbak = tr->obj->pos;
|
||||
dReal *Rbak = tr->obj->R;
|
||||
dxBody *bodybak = tr->obj->body;
|
||||
|
||||
// compute temporary pos and R for the encapsulated geom object
|
||||
if (!o1->space_aabb) compute_final_tx (o1);
|
||||
tr->obj->pos = tr->final_pos;
|
||||
tr->obj->R = tr->final_R;
|
||||
tr->obj->body = o1->body;
|
||||
|
||||
// do the collision
|
||||
int n = dCollide (tr->obj,const_cast<dxGeom*>(o2),flags,contact,skip);
|
||||
|
||||
// if required, adjust the 'g1' values in the generated contacts so that
|
||||
// thay indicated the GeomTransform object instead of the encapsulated
|
||||
// object.
|
||||
if (tr->infomode) {
|
||||
for (int i=0; i<n; i++) {
|
||||
dContactGeom *c = CONTACT(contact,skip*i);
|
||||
c->g1 = const_cast<dxGeom*> (o1);
|
||||
}
|
||||
}
|
||||
|
||||
// restore the pos, R and body
|
||||
tr->obj->pos = posbak;
|
||||
tr->obj->R = Rbak;
|
||||
tr->obj->body = bodybak;
|
||||
return n;
|
||||
}
|
||||
|
||||
|
||||
static dColliderFn * dGeomTransformColliderFn (int num)
|
||||
{
|
||||
return (dColliderFn *) &dCollideT;
|
||||
}
|
||||
|
||||
|
||||
static void dGeomTransformAABB (dxGeom *geom, dReal aabb[6])
|
||||
{
|
||||
dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(geom);
|
||||
if (!tr->obj) {
|
||||
dSetZero (aabb,6);
|
||||
return;
|
||||
}
|
||||
|
||||
// backup the relative pos and R pointers of the encapsulated geom object
|
||||
dReal *posbak = tr->obj->pos;
|
||||
dReal *Rbak = tr->obj->R;
|
||||
|
||||
// compute temporary pos and R for the encapsulated geom object
|
||||
compute_final_tx (geom);
|
||||
tr->obj->pos = tr->final_pos;
|
||||
tr->obj->R = tr->final_R;
|
||||
|
||||
// compute the AABB
|
||||
tr->obj->_class->aabb (tr->obj,aabb);
|
||||
|
||||
// restore the pos and R
|
||||
tr->obj->pos = posbak;
|
||||
tr->obj->R = Rbak;
|
||||
}
|
||||
|
||||
|
||||
static void dGeomTransformDtor (dxGeom *geom)
|
||||
{
|
||||
dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(geom);
|
||||
if (tr->obj && tr->cleanup) {
|
||||
dGeomDestroy (tr->obj);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
dxGeom *dCreateGeomTransform (dSpaceID space)
|
||||
{
|
||||
if (dGeomTransformClass == -1) {
|
||||
dGeomClass c;
|
||||
c.bytes = sizeof (dxGeomTransform);
|
||||
c.collider = &dGeomTransformColliderFn;
|
||||
c.aabb = &dGeomTransformAABB;
|
||||
c.aabb_test = 0;
|
||||
c.dtor = dGeomTransformDtor;
|
||||
dGeomTransformClass = dCreateGeomClass (&c);
|
||||
}
|
||||
|
||||
dxGeom *g = dCreateGeom (dGeomTransformClass);
|
||||
if (space) dSpaceAdd (space,g);
|
||||
|
||||
dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(g);
|
||||
tr->obj = 0;
|
||||
tr->cleanup = 0;
|
||||
tr->infomode = 0;
|
||||
dSetZero (tr->final_pos,4);
|
||||
dRSetIdentity (tr->final_R);
|
||||
|
||||
return g;
|
||||
}
|
||||
|
||||
|
||||
void dGeomTransformSetGeom (dxGeom *g, dxGeom *obj)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dGeomTransformClass,
|
||||
"argument not a geom transform");
|
||||
dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(g);
|
||||
if (tr->obj && tr->cleanup) {
|
||||
dGeomDestroy (tr->obj);
|
||||
}
|
||||
tr->obj = obj;
|
||||
}
|
||||
|
||||
|
||||
dxGeom * dGeomTransformGetGeom (dxGeom *g)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dGeomTransformClass,
|
||||
"argument not a geom transform");
|
||||
dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(g);
|
||||
return tr->obj;
|
||||
}
|
||||
|
||||
|
||||
void dGeomTransformSetCleanup (dGeomID g, int mode)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dGeomTransformClass,
|
||||
"argument not a geom transform");
|
||||
dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(g);
|
||||
tr->cleanup = mode;
|
||||
}
|
||||
|
||||
|
||||
int dGeomTransformGetCleanup (dGeomID g)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dGeomTransformClass,
|
||||
"argument not a geom transform");
|
||||
dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(g);
|
||||
return tr->cleanup;
|
||||
}
|
||||
|
||||
|
||||
void dGeomTransformSetInfo (dGeomID g, int mode)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dGeomTransformClass,
|
||||
"argument not a geom transform");
|
||||
dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(g);
|
||||
tr->infomode = mode;
|
||||
}
|
||||
|
||||
|
||||
int dGeomTransformGetInfo (dGeomID g)
|
||||
{
|
||||
dUASSERT (g && g->_class->num == dGeomTransformClass,
|
||||
"argument not a geom transform");
|
||||
dxGeomTransform *tr = (dxGeomTransform*) CLASSDATA(g);
|
||||
return tr->infomode;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// other utility functions
|
||||
|
||||
void dInfiniteAABB (dxGeom *geom, dReal aabb[6])
|
||||
{
|
||||
aabb[0] = -dInfinity;
|
||||
aabb[1] = dInfinity;
|
||||
aabb[2] = -dInfinity;
|
||||
aabb[3] = dInfinity;
|
||||
aabb[4] = -dInfinity;
|
||||
aabb[5] = dInfinity;
|
||||
}
|
||||
|
||||
|
||||
void dCloseODE()
|
||||
{
|
||||
if (colliders) {
|
||||
delete colliders;
|
||||
colliders = 0;
|
||||
}
|
||||
if (classes) {
|
||||
for (int i=0; i < classes->size(); i++) {
|
||||
dFree ((*classes)[i], sizeof (dxGeomClass));
|
||||
}
|
||||
delete classes;
|
||||
classes = 0;
|
||||
}
|
||||
|
||||
// reset geom class vars
|
||||
dSphereClass = -1;
|
||||
dBoxClass = -1;
|
||||
dCCylinderClass = -1;
|
||||
dPlaneClass = -1;
|
||||
dGeomGroupClass = -1;
|
||||
dGeomTransformClass = -1;
|
||||
|
||||
// if you're using contrib code you may want to uncomment the following:
|
||||
// dTriListClass = -1;
|
||||
// dRayClass = -1;
|
||||
}
|
||||
83
extern/ode/dist/ode/src/geom_internal.h
vendored
83
extern/ode/dist/ode/src/geom_internal.h
vendored
@@ -1,83 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef _ODE_GEOM_INTERNAL_H_
|
||||
#define _ODE_GEOM_INTERNAL_H_
|
||||
|
||||
|
||||
// mask for the number-of-contacts field in the dCollide() flags parameter
|
||||
#define NUMC_MASK (0xffff)
|
||||
|
||||
|
||||
// internal info for geometry class
|
||||
|
||||
struct dxGeomClass {
|
||||
dGetColliderFnFn *collider;
|
||||
dGetAABBFn *aabb;
|
||||
dAABBTestFn *aabb_test;
|
||||
dGeomDtorFn *dtor;
|
||||
int num; // class number
|
||||
int size; // total size of object, including extra data area
|
||||
};
|
||||
|
||||
|
||||
// position vector and rotation matrix for geometry objects that are not
|
||||
// connected to bodies.
|
||||
|
||||
struct dxPosR {
|
||||
dVector3 pos;
|
||||
dMatrix3 R;
|
||||
};
|
||||
|
||||
|
||||
// common data for all geometry objects. the class-specific data area follows
|
||||
// this structure. pos and R will either point to a separately allocated
|
||||
// buffer (if body is 0 - pos points to the dxPosR object) or to the pos and
|
||||
// R of the body (if body nonzero).
|
||||
|
||||
struct dxGeom { // a dGeomID is a pointer to this
|
||||
dxGeomClass *_class; // class of this object
|
||||
void *data; // user data pointer
|
||||
dBodyID body; // dynamics body associated with this object (if any)
|
||||
dReal *pos; // pointer to object's position vector
|
||||
dReal *R; // pointer to object's rotation matrix
|
||||
dSpaceID spaceid; // the space this object is in
|
||||
dGeomSpaceData space; // reserved for use by space this object is in
|
||||
dReal *space_aabb; // ptr to aabb array held by dSpaceCollide() fn
|
||||
// class-specific data follows here, with proper alignment.
|
||||
};
|
||||
|
||||
|
||||
// this is the size of the dxGeom structure rounded up to a multiple of 16
|
||||
// bytes. any class specific data that comes after this will have the correct
|
||||
// alignment.
|
||||
|
||||
#define SIZEOF_DXGEOM dEFFICIENT_SIZE(sizeof(dxGeom))
|
||||
|
||||
|
||||
// given a pointer to a dxGeom, return a pointer to the class data that
|
||||
// follows it.
|
||||
|
||||
#define CLASSDATA(geomptr) (((char*)geomptr) + SIZEOF_DXGEOM)
|
||||
|
||||
#endif
|
||||
|
||||
2160
extern/ode/dist/ode/src/joint.cpp
vendored
2160
extern/ode/dist/ode/src/joint.cpp
vendored
@@ -1,2160 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
/*
|
||||
|
||||
design note: the general principle for giving a joint the option of connecting
|
||||
to the static environment (i.e. the absolute frame) is to check the second
|
||||
body (joint->node[1].body), and if it is zero then behave as if its body
|
||||
transform is the identity.
|
||||
|
||||
*/
|
||||
|
||||
#include <ode/odemath.h>
|
||||
#include <ode/rotation.h>
|
||||
#include <ode/matrix.h>
|
||||
#include "joint.h"
|
||||
|
||||
//****************************************************************************
|
||||
// externs
|
||||
|
||||
extern "C" void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz);
|
||||
extern "C" void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz);
|
||||
|
||||
//****************************************************************************
|
||||
// utility
|
||||
|
||||
// set three "ball-and-socket" rows in the constraint equation, and the
|
||||
// corresponding right hand side.
|
||||
|
||||
static inline void setBall (dxJoint *joint, dxJoint::Info2 *info,
|
||||
dVector3 anchor1, dVector3 anchor2)
|
||||
{
|
||||
// anchor points in global coordinates with respect to body PORs.
|
||||
dVector3 a1,a2;
|
||||
|
||||
int s = info->rowskip;
|
||||
|
||||
// set jacobian
|
||||
info->J1l[0] = 1;
|
||||
info->J1l[s+1] = 1;
|
||||
info->J1l[2*s+2] = 1;
|
||||
dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1);
|
||||
dCROSSMAT (info->J1a,a1,s,-,+);
|
||||
if (joint->node[1].body) {
|
||||
info->J2l[0] = -1;
|
||||
info->J2l[s+1] = -1;
|
||||
info->J2l[2*s+2] = -1;
|
||||
dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2);
|
||||
dCROSSMAT (info->J2a,a2,s,+,-);
|
||||
}
|
||||
|
||||
// set right hand side
|
||||
dReal k = info->fps * info->erp;
|
||||
if (joint->node[1].body) {
|
||||
for (int j=0; j<3; j++) {
|
||||
info->c[j] = k * (a2[j] + joint->node[1].body->pos[j] -
|
||||
a1[j] - joint->node[0].body->pos[j]);
|
||||
}
|
||||
}
|
||||
else {
|
||||
for (int j=0; j<3; j++) {
|
||||
info->c[j] = k * (anchor2[j] - a1[j] -
|
||||
joint->node[0].body->pos[j]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// this is like setBall(), except that `axis' is a unit length vector
|
||||
// (in global coordinates) that should be used for the first jacobian
|
||||
// position row (the other two row vectors will be derived from this).
|
||||
// `erp1' is the erp value to use along the axis.
|
||||
|
||||
static inline void setBall2 (dxJoint *joint, dxJoint::Info2 *info,
|
||||
dVector3 anchor1, dVector3 anchor2,
|
||||
dVector3 axis, dReal erp1)
|
||||
{
|
||||
// anchor points in global coordinates with respect to body PORs.
|
||||
dVector3 a1,a2;
|
||||
|
||||
int i,s = info->rowskip;
|
||||
|
||||
// get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0],
|
||||
// [0 1 0] and [0 0 1], which makes everything much easier.
|
||||
dVector3 q1,q2;
|
||||
dPlaneSpace (axis,q1,q2);
|
||||
|
||||
// set jacobian
|
||||
for (i=0; i<3; i++) info->J1l[i] = axis[i];
|
||||
for (i=0; i<3; i++) info->J1l[s+i] = q1[i];
|
||||
for (i=0; i<3; i++) info->J1l[2*s+i] = q2[i];
|
||||
dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1);
|
||||
dCROSS (info->J1a,=,a1,axis);
|
||||
dCROSS (info->J1a+s,=,a1,q1);
|
||||
dCROSS (info->J1a+2*s,=,a1,q2);
|
||||
if (joint->node[1].body) {
|
||||
for (i=0; i<3; i++) info->J2l[i] = -axis[i];
|
||||
for (i=0; i<3; i++) info->J2l[s+i] = -q1[i];
|
||||
for (i=0; i<3; i++) info->J2l[2*s+i] = -q2[i];
|
||||
dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2);
|
||||
dCROSS (info->J2a,= -,a2,axis);
|
||||
dCROSS (info->J2a+s,= -,a2,q1);
|
||||
dCROSS (info->J2a+2*s,= -,a2,q2);
|
||||
}
|
||||
|
||||
// set right hand side - measure error along (axis,q1,q2)
|
||||
dReal k1 = info->fps * erp1;
|
||||
dReal k = info->fps * info->erp;
|
||||
|
||||
for (i=0; i<3; i++) a1[i] += joint->node[0].body->pos[i];
|
||||
if (joint->node[1].body) {
|
||||
for (i=0; i<3; i++) a2[i] += joint->node[1].body->pos[i];
|
||||
info->c[0] = k1 * (dDOT(axis,a2) - dDOT(axis,a1));
|
||||
info->c[1] = k * (dDOT(q1,a2) - dDOT(q1,a1));
|
||||
info->c[2] = k * (dDOT(q2,a2) - dDOT(q2,a1));
|
||||
}
|
||||
else {
|
||||
info->c[0] = k1 * (dDOT(axis,anchor2) - dDOT(axis,a1));
|
||||
info->c[1] = k * (dDOT(q1,anchor2) - dDOT(q1,a1));
|
||||
info->c[2] = k * (dDOT(q2,anchor2) - dDOT(q2,a1));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// compute anchor points relative to bodies
|
||||
|
||||
static void setAnchors (dxJoint *j, dReal x, dReal y, dReal z,
|
||||
dVector3 anchor1, dVector3 anchor2)
|
||||
{
|
||||
if (j->node[0].body) {
|
||||
dReal q[4];
|
||||
q[0] = x - j->node[0].body->pos[0];
|
||||
q[1] = y - j->node[0].body->pos[1];
|
||||
q[2] = z - j->node[0].body->pos[2];
|
||||
q[3] = 0;
|
||||
dMULTIPLY1_331 (anchor1,j->node[0].body->R,q);
|
||||
if (j->node[1].body) {
|
||||
q[0] = x - j->node[1].body->pos[0];
|
||||
q[1] = y - j->node[1].body->pos[1];
|
||||
q[2] = z - j->node[1].body->pos[2];
|
||||
q[3] = 0;
|
||||
dMULTIPLY1_331 (anchor2,j->node[1].body->R,q);
|
||||
}
|
||||
else {
|
||||
anchor2[0] = x;
|
||||
anchor2[1] = y;
|
||||
anchor2[2] = z;
|
||||
}
|
||||
}
|
||||
anchor1[3] = 0;
|
||||
anchor2[3] = 0;
|
||||
}
|
||||
|
||||
|
||||
// compute axes relative to bodies. axis2 can be 0
|
||||
|
||||
static void setAxes (dxJoint *j, dReal x, dReal y, dReal z,
|
||||
dVector3 axis1, dVector3 axis2)
|
||||
{
|
||||
if (j->node[0].body) {
|
||||
dReal q[4];
|
||||
q[0] = x;
|
||||
q[1] = y;
|
||||
q[2] = z;
|
||||
q[3] = 0;
|
||||
dNormalize3 (q);
|
||||
dMULTIPLY1_331 (axis1,j->node[0].body->R,q);
|
||||
if (axis2) {
|
||||
if (j->node[1].body) {
|
||||
dMULTIPLY1_331 (axis2,j->node[1].body->R,q);
|
||||
}
|
||||
else {
|
||||
axis2[0] = x;
|
||||
axis2[1] = y;
|
||||
axis2[2] = z;
|
||||
}
|
||||
axis2[3] = 0;
|
||||
}
|
||||
}
|
||||
axis1[3] = 0;
|
||||
}
|
||||
|
||||
|
||||
static void getAnchor (dxJoint *j, dVector3 result, dVector3 anchor1)
|
||||
{
|
||||
if (j->node[0].body) {
|
||||
dMULTIPLY0_331 (result,j->node[0].body->R,anchor1);
|
||||
result[0] += j->node[0].body->pos[0];
|
||||
result[1] += j->node[0].body->pos[1];
|
||||
result[2] += j->node[0].body->pos[2];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void getAxis (dxJoint *j, dVector3 result, dVector3 axis1)
|
||||
{
|
||||
if (j->node[0].body) {
|
||||
dMULTIPLY0_331 (result,j->node[0].body->R,axis1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// given two bodies (body1,body2), the hinge axis that they are connected by
|
||||
// w.r.t. body1 (axis), and the initial relative orientation between them
|
||||
// (q_initial), return the relative rotation angle. the initial relative
|
||||
// orientation corresponds to an angle of zero. if body2 is 0 then measure the
|
||||
// angle between body1 and the static frame.
|
||||
//
|
||||
// this will not return the correct angle if the bodies rotate along any axis
|
||||
// other than the given hinge axis.
|
||||
|
||||
static dReal getHingeAngle (dxBody *body1, dxBody *body2, dVector3 axis,
|
||||
dQuaternion q_initial)
|
||||
{
|
||||
// the angle between the two bodies is extracted from the quaternion that
|
||||
// represents the relative rotation between them. recall that a quaternion
|
||||
// q is:
|
||||
// [s,v] = [ cos(theta/2) , sin(theta/2) * u ]
|
||||
// where s is a scalar and v is a 3-vector. u is a unit length axis and
|
||||
// theta is a rotation along that axis. we can get theta/2 by:
|
||||
// theta/2 = atan2 ( sin(theta/2) , cos(theta/2) )
|
||||
// but we can't get sin(theta/2) directly, only its absolute value, i.e.:
|
||||
// |v| = |sin(theta/2)| * |u|
|
||||
// = |sin(theta/2)|
|
||||
// using this value will have a strange effect. recall that there are two
|
||||
// quaternion representations of a given rotation, q and -q. typically as
|
||||
// a body rotates along the axis it will go through a complete cycle using
|
||||
// one representation and then the next cycle will use the other
|
||||
// representation. this corresponds to u pointing in the direction of the
|
||||
// hinge axis and then in the opposite direction. the result is that theta
|
||||
// will appear to go "backwards" every other cycle. here is a fix: if u
|
||||
// points "away" from the direction of the hinge (motor) axis (i.e. more
|
||||
// than 90 degrees) then use -q instead of q. this represents the same
|
||||
// rotation, but results in the cos(theta/2) value being sign inverted.
|
||||
|
||||
// get qrel = relative rotation between the two bodies
|
||||
dQuaternion qrel;
|
||||
if (body2) {
|
||||
dQuaternion qq;
|
||||
dQMultiply1 (qq,body1->q,body2->q);
|
||||
dQMultiply2 (qrel,qq,q_initial);
|
||||
}
|
||||
else {
|
||||
// pretend body2->q is the identity
|
||||
dQMultiply3 (qrel,body1->q,q_initial);
|
||||
}
|
||||
|
||||
// extract the angle from the quaternion. cost2 = cos(theta/2),
|
||||
// sint2 = |sin(theta/2)|
|
||||
dReal cost2 = qrel[0];
|
||||
dReal sint2 = dSqrt (qrel[1]*qrel[1]+qrel[2]*qrel[2]+qrel[3]*qrel[3]);
|
||||
dReal theta = (dDOT(qrel+1,axis) >= 0) ? // @@@ padding assumptions
|
||||
(2 * dAtan2(sint2,cost2)) : // if u points in direction of axis
|
||||
(2 * dAtan2(sint2,-cost2)); // if u points in opposite direction
|
||||
|
||||
// the angle we get will be between 0..2*pi, but we want to return angles
|
||||
// between -pi..pi
|
||||
if (theta > M_PI) theta -= 2*M_PI;
|
||||
|
||||
// the angle we've just extracted has the wrong sign
|
||||
theta = -theta;
|
||||
|
||||
return theta;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// dxJointLimitMotor
|
||||
|
||||
void dxJointLimitMotor::init (dxWorld *world)
|
||||
{
|
||||
vel = 0;
|
||||
fmax = 0;
|
||||
lostop = -dInfinity;
|
||||
histop = dInfinity;
|
||||
fudge_factor = 1;
|
||||
normal_cfm = world->global_cfm;
|
||||
stop_erp = world->global_erp;
|
||||
stop_cfm = world->global_cfm;
|
||||
bounce = 0;
|
||||
limit = 0;
|
||||
limit_err = 0;
|
||||
}
|
||||
|
||||
|
||||
void dxJointLimitMotor::set (int num, dReal value)
|
||||
{
|
||||
switch (num) {
|
||||
case dParamLoStop:
|
||||
if (value <= histop) lostop = value;
|
||||
break;
|
||||
case dParamHiStop:
|
||||
if (value >= lostop) histop = value;
|
||||
break;
|
||||
case dParamVel:
|
||||
vel = value;
|
||||
break;
|
||||
case dParamFMax:
|
||||
if (value >= 0) fmax = value;
|
||||
break;
|
||||
case dParamFudgeFactor:
|
||||
if (value >= 0 && value <= 1) fudge_factor = value;
|
||||
break;
|
||||
case dParamBounce:
|
||||
bounce = value;
|
||||
break;
|
||||
case dParamCFM:
|
||||
normal_cfm = value;
|
||||
break;
|
||||
case dParamStopERP:
|
||||
stop_erp = value;
|
||||
break;
|
||||
case dParamStopCFM:
|
||||
stop_cfm = value;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
dReal dxJointLimitMotor::get (int num)
|
||||
{
|
||||
switch (num) {
|
||||
case dParamLoStop: return lostop;
|
||||
case dParamHiStop: return histop;
|
||||
case dParamVel: return vel;
|
||||
case dParamFMax: return fmax;
|
||||
case dParamFudgeFactor: return fudge_factor;
|
||||
case dParamBounce: return bounce;
|
||||
case dParamCFM: return normal_cfm;
|
||||
case dParamStopERP: return stop_erp;
|
||||
case dParamStopCFM: return stop_cfm;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int dxJointLimitMotor::testRotationalLimit (dReal angle)
|
||||
{
|
||||
if (angle <= lostop) {
|
||||
limit = 1;
|
||||
limit_err = angle - lostop;
|
||||
return 1;
|
||||
}
|
||||
else if (angle >= histop) {
|
||||
limit = 2;
|
||||
limit_err = angle - histop;
|
||||
return 1;
|
||||
}
|
||||
else {
|
||||
limit = 0;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int dxJointLimitMotor::addLimot (dxJoint *joint,
|
||||
dxJoint::Info2 *info, int row,
|
||||
dVector3 ax1, int rotational)
|
||||
{
|
||||
int srow = row * info->rowskip;
|
||||
|
||||
// if the joint is powered, or has joint limits, add in the extra row
|
||||
int powered = fmax > 0;
|
||||
if (powered || limit) {
|
||||
dReal *J1 = rotational ? info->J1a : info->J1l;
|
||||
dReal *J2 = rotational ? info->J2a : info->J2l;
|
||||
|
||||
J1[srow+0] = ax1[0];
|
||||
J1[srow+1] = ax1[1];
|
||||
J1[srow+2] = ax1[2];
|
||||
if (joint->node[1].body) {
|
||||
J2[srow+0] = -ax1[0];
|
||||
J2[srow+1] = -ax1[1];
|
||||
J2[srow+2] = -ax1[2];
|
||||
}
|
||||
|
||||
// if we're limited low and high simultaneously, the joint motor is
|
||||
// ineffective
|
||||
if (limit && (lostop == histop)) powered = 0;
|
||||
|
||||
if (powered) {
|
||||
info->cfm[row] = normal_cfm;
|
||||
if (! limit) {
|
||||
info->c[row] = vel;
|
||||
info->lo[row] = -fmax;
|
||||
info->hi[row] = fmax;
|
||||
}
|
||||
else {
|
||||
// the joint is at a limit, AND is being powered. if the joint is
|
||||
// being powered into the limit then we apply the maximum motor force
|
||||
// in that direction, because the motor is working against the
|
||||
// immovable limit. if the joint is being powered away from the limit
|
||||
// then we have problems because actually we need *two* lcp
|
||||
// constraints to handle this case. so we fake it and apply some
|
||||
// fraction of the maximum force. the fraction to use can be set as
|
||||
// a fudge factor.
|
||||
|
||||
dReal fm = fmax;
|
||||
if (vel > 0) fm = -fm;
|
||||
|
||||
// if we're powering away from the limit, apply the fudge factor
|
||||
if ((limit==1 && vel > 0) || (limit==2 && vel < 0)) fm *= fudge_factor;
|
||||
|
||||
if (rotational) {
|
||||
dBodyAddTorque (joint->node[0].body,-fm*ax1[0],-fm*ax1[1],
|
||||
-fm*ax1[2]);
|
||||
if (joint->node[1].body)
|
||||
dBodyAddTorque (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]);
|
||||
}
|
||||
else {
|
||||
dBodyAddForce (joint->node[0].body,-fm*ax1[0],-fm*ax1[1],-fm*ax1[2]);
|
||||
if (joint->node[1].body)
|
||||
dBodyAddForce (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (limit) {
|
||||
dReal k = info->fps * stop_erp;
|
||||
info->c[row] = -k * limit_err;
|
||||
info->cfm[row] = stop_cfm;
|
||||
|
||||
if (lostop == histop) {
|
||||
// limited low and high simultaneously
|
||||
info->lo[row] = -dInfinity;
|
||||
info->hi[row] = dInfinity;
|
||||
}
|
||||
else {
|
||||
if (limit == 1) {
|
||||
// low limit
|
||||
info->lo[row] = 0;
|
||||
info->hi[row] = dInfinity;
|
||||
}
|
||||
else {
|
||||
// high limit
|
||||
info->lo[row] = -dInfinity;
|
||||
info->hi[row] = 0;
|
||||
}
|
||||
|
||||
// deal with bounce
|
||||
if (bounce > 0) {
|
||||
// calculate joint velocity
|
||||
dReal vel;
|
||||
if (rotational) {
|
||||
vel = dDOT(joint->node[0].body->avel,ax1);
|
||||
if (joint->node[1].body)
|
||||
vel -= dDOT(joint->node[1].body->avel,ax1);
|
||||
}
|
||||
else {
|
||||
vel = dDOT(joint->node[0].body->lvel,ax1);
|
||||
if (joint->node[1].body)
|
||||
vel -= dDOT(joint->node[1].body->lvel,ax1);
|
||||
}
|
||||
|
||||
// only apply bounce if the velocity is incoming, and if the
|
||||
// resulting c[] exceeds what we already have.
|
||||
if (limit == 1) {
|
||||
// low limit
|
||||
if (vel < 0) {
|
||||
dReal newc = -bounce * vel;
|
||||
if (newc > info->c[row]) info->c[row] = newc;
|
||||
}
|
||||
}
|
||||
else {
|
||||
// high limit - all those computations are reversed
|
||||
if (vel > 0) {
|
||||
dReal newc = -bounce * vel;
|
||||
if (newc < info->c[row]) info->c[row] = newc;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
else return 0;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// ball and socket
|
||||
|
||||
static void ballInit (dxJointBall *j)
|
||||
{
|
||||
dSetZero (j->anchor1,4);
|
||||
dSetZero (j->anchor2,4);
|
||||
}
|
||||
|
||||
|
||||
static void ballGetInfo1 (dxJointBall *j, dxJoint::Info1 *info)
|
||||
{
|
||||
info->m = 3;
|
||||
info->nub = 3;
|
||||
}
|
||||
|
||||
|
||||
static void ballGetInfo2 (dxJointBall *joint, dxJoint::Info2 *info)
|
||||
{
|
||||
setBall (joint,info,joint->anchor1,joint->anchor2);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetBallAnchor (dxJointBall *joint,
|
||||
dReal x, dReal y, dReal z)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball");
|
||||
setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointGetBallAnchor (dxJointBall *joint, dVector3 result)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(result,"bad result argument");
|
||||
dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball");
|
||||
getAnchor (joint,result,joint->anchor1);
|
||||
}
|
||||
|
||||
|
||||
dxJoint::Vtable __dball_vtable = {
|
||||
sizeof(dxJointBall),
|
||||
(dxJoint::init_fn*) ballInit,
|
||||
(dxJoint::getInfo1_fn*) ballGetInfo1,
|
||||
(dxJoint::getInfo2_fn*) ballGetInfo2,
|
||||
dJointTypeBall};
|
||||
|
||||
//****************************************************************************
|
||||
// hinge
|
||||
|
||||
static void hingeInit (dxJointHinge *j)
|
||||
{
|
||||
dSetZero (j->anchor1,4);
|
||||
dSetZero (j->anchor2,4);
|
||||
dSetZero (j->axis1,4);
|
||||
j->axis1[0] = 1;
|
||||
dSetZero (j->axis2,4);
|
||||
j->axis2[0] = 1;
|
||||
dSetZero (j->qrel,4);
|
||||
j->limot.init (j->world);
|
||||
}
|
||||
|
||||
|
||||
static void hingeGetInfo1 (dxJointHinge *j, dxJoint::Info1 *info)
|
||||
{
|
||||
info->nub = 5;
|
||||
|
||||
// see if joint is powered
|
||||
if (j->limot.fmax > 0)
|
||||
info->m = 6; // powered hinge needs an extra constraint row
|
||||
else info->m = 5;
|
||||
|
||||
// see if we're at a joint limit.
|
||||
if ((j->limot.lostop >= -M_PI || j->limot.histop <= M_PI) &&
|
||||
j->limot.lostop <= j->limot.histop) {
|
||||
dReal angle = getHingeAngle (j->node[0].body,j->node[1].body,j->axis1,
|
||||
j->qrel);
|
||||
if (j->limot.testRotationalLimit (angle)) info->m = 6;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void hingeGetInfo2 (dxJointHinge *joint, dxJoint::Info2 *info)
|
||||
{
|
||||
// set the three ball-and-socket rows
|
||||
setBall (joint,info,joint->anchor1,joint->anchor2);
|
||||
|
||||
// set the two hinge rows. the hinge axis should be the only unconstrained
|
||||
// rotational axis, the angular velocity of the two bodies perpendicular to
|
||||
// the hinge axis should be equal. thus the constraint equations are
|
||||
// p*w1 - p*w2 = 0
|
||||
// q*w1 - q*w2 = 0
|
||||
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
|
||||
// are the angular velocity vectors of the two bodies.
|
||||
|
||||
dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body
|
||||
dVector3 p,q; // plane space vectors for ax1
|
||||
dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
|
||||
dPlaneSpace (ax1,p,q);
|
||||
|
||||
int s3=3*info->rowskip;
|
||||
int s4=4*info->rowskip;
|
||||
|
||||
info->J1a[s3+0] = p[0];
|
||||
info->J1a[s3+1] = p[1];
|
||||
info->J1a[s3+2] = p[2];
|
||||
info->J1a[s4+0] = q[0];
|
||||
info->J1a[s4+1] = q[1];
|
||||
info->J1a[s4+2] = q[2];
|
||||
|
||||
if (joint->node[1].body) {
|
||||
info->J2a[s3+0] = -p[0];
|
||||
info->J2a[s3+1] = -p[1];
|
||||
info->J2a[s3+2] = -p[2];
|
||||
info->J2a[s4+0] = -q[0];
|
||||
info->J2a[s4+1] = -q[1];
|
||||
info->J2a[s4+2] = -q[2];
|
||||
}
|
||||
|
||||
// compute the right hand side of the constraint equation. set relative
|
||||
// body velocities along p and q to bring the hinge back into alignment.
|
||||
// if ax1,ax2 are the unit length hinge axes as computed from body1 and
|
||||
// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
|
||||
// if `theta' is the angle between ax1 and ax2, we need an angular velocity
|
||||
// along u to cover angle erp*theta in one step :
|
||||
// |angular_velocity| = angle/time = erp*theta / stepsize
|
||||
// = (erp*fps) * theta
|
||||
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
|
||||
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
|
||||
// ...as ax1 and ax2 are unit length. if theta is smallish,
|
||||
// theta ~= sin(theta), so
|
||||
// angular_velocity = (erp*fps) * (ax1 x ax2)
|
||||
// ax1 x ax2 is in the plane space of ax1, so we project the angular
|
||||
// velocity to p and q to find the right hand side.
|
||||
|
||||
dVector3 ax2,b;
|
||||
if (joint->node[1].body) {
|
||||
dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2);
|
||||
}
|
||||
else {
|
||||
ax2[0] = joint->axis2[0];
|
||||
ax2[1] = joint->axis2[1];
|
||||
ax2[2] = joint->axis2[2];
|
||||
}
|
||||
dCROSS (b,=,ax1,ax2);
|
||||
dReal k = info->fps * info->erp;
|
||||
info->c[3] = k * dDOT(b,p);
|
||||
info->c[4] = k * dDOT(b,q);
|
||||
|
||||
// if the hinge is powered, or has joint limits, add in the stuff
|
||||
joint->limot.addLimot (joint,info,5,ax1,1);
|
||||
}
|
||||
|
||||
|
||||
// compute initial relative rotation body1 -> body2, or env -> body1
|
||||
|
||||
static void hingeComputeInitialRelativeRotation (dxJointHinge *joint)
|
||||
{
|
||||
if (joint->node[0].body) {
|
||||
if (joint->node[1].body) {
|
||||
dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q);
|
||||
}
|
||||
else {
|
||||
// set joint->qrel to the transpose of the first body q
|
||||
joint->qrel[0] = joint->node[0].body->q[0];
|
||||
for (int i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetHingeAnchor (dxJointHinge *joint,
|
||||
dReal x, dReal y, dReal z)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
|
||||
setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
|
||||
hingeComputeInitialRelativeRotation (joint);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetHingeAxis (dxJointHinge *joint,
|
||||
dReal x, dReal y, dReal z)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
|
||||
setAxes (joint,x,y,z,joint->axis1,joint->axis2);
|
||||
hingeComputeInitialRelativeRotation (joint);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointGetHingeAnchor (dxJointHinge *joint, dVector3 result)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(result,"bad result argument");
|
||||
dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
|
||||
getAnchor (joint,result,joint->anchor1);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointGetHingeAxis (dxJointHinge *joint, dVector3 result)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(result,"bad result argument");
|
||||
dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
|
||||
getAxis (joint,result,joint->axis1);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetHingeParam (dxJointHinge *joint,
|
||||
int parameter, dReal value)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
|
||||
joint->limot.set (parameter,value);
|
||||
}
|
||||
|
||||
|
||||
extern "C" dReal dJointGetHingeParam (dxJointHinge *joint, int parameter)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
|
||||
return joint->limot.get (parameter);
|
||||
}
|
||||
|
||||
|
||||
extern "C" dReal dJointGetHingeAngle (dxJointHinge *joint)
|
||||
{
|
||||
dAASSERT(joint);
|
||||
dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
|
||||
if (joint->node[0].body) {
|
||||
return getHingeAngle (joint->node[0].body,joint->node[1].body,joint->axis1,
|
||||
joint->qrel);
|
||||
}
|
||||
else return 0;
|
||||
}
|
||||
|
||||
|
||||
extern "C" dReal dJointGetHingeAngleRate (dxJointHinge *joint)
|
||||
{
|
||||
dAASSERT(joint);
|
||||
dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge");
|
||||
if (joint->node[0].body) {
|
||||
dVector3 axis;
|
||||
dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1);
|
||||
dReal rate = dDOT(axis,joint->node[0].body->avel);
|
||||
if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel);
|
||||
return rate;
|
||||
}
|
||||
else return 0;
|
||||
}
|
||||
|
||||
|
||||
dxJoint::Vtable __dhinge_vtable = {
|
||||
sizeof(dxJointHinge),
|
||||
(dxJoint::init_fn*) hingeInit,
|
||||
(dxJoint::getInfo1_fn*) hingeGetInfo1,
|
||||
(dxJoint::getInfo2_fn*) hingeGetInfo2,
|
||||
dJointTypeHinge};
|
||||
|
||||
//****************************************************************************
|
||||
// slider
|
||||
|
||||
static void sliderInit (dxJointSlider *j)
|
||||
{
|
||||
dSetZero (j->axis1,4);
|
||||
j->axis1[0] = 1;
|
||||
dSetZero (j->qrel,4);
|
||||
dSetZero (j->offset,4);
|
||||
j->limot.init (j->world);
|
||||
}
|
||||
|
||||
|
||||
extern "C" dReal dJointGetSliderPosition (dxJointSlider *joint)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
|
||||
|
||||
// get axis1 in global coordinates
|
||||
dVector3 ax1,q;
|
||||
dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
|
||||
|
||||
if (joint->node[1].body) {
|
||||
// get body2 + offset point in global coordinates
|
||||
dMULTIPLY0_331 (q,joint->node[1].body->R,joint->offset);
|
||||
for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] - q[i] -
|
||||
joint->node[1].body->pos[i];
|
||||
}
|
||||
else {
|
||||
for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] -
|
||||
joint->offset[i];
|
||||
|
||||
}
|
||||
return dDOT(ax1,q);
|
||||
}
|
||||
|
||||
|
||||
extern "C" dReal dJointGetSliderPositionRate (dxJointSlider *joint)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
|
||||
|
||||
// get axis1 in global coordinates
|
||||
dVector3 ax1;
|
||||
dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
|
||||
|
||||
if (joint->node[1].body) {
|
||||
return dDOT(ax1,joint->node[0].body->lvel) -
|
||||
dDOT(ax1,joint->node[1].body->lvel);
|
||||
}
|
||||
else {
|
||||
return dDOT(ax1,joint->node[0].body->lvel);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void sliderGetInfo1 (dxJointSlider *j, dxJoint::Info1 *info)
|
||||
{
|
||||
info->nub = 5;
|
||||
|
||||
// see if joint is powered
|
||||
if (j->limot.fmax > 0)
|
||||
info->m = 6; // powered slider needs an extra constraint row
|
||||
else info->m = 5;
|
||||
|
||||
// see if we're at a joint limit.
|
||||
j->limot.limit = 0;
|
||||
if ((j->limot.lostop > -dInfinity || j->limot.histop < dInfinity) &&
|
||||
j->limot.lostop <= j->limot.histop) {
|
||||
// measure joint position
|
||||
dReal pos = dJointGetSliderPosition (j);
|
||||
if (pos <= j->limot.lostop) {
|
||||
j->limot.limit = 1;
|
||||
j->limot.limit_err = pos - j->limot.lostop;
|
||||
info->m = 6;
|
||||
}
|
||||
else if (pos >= j->limot.histop) {
|
||||
j->limot.limit = 2;
|
||||
j->limot.limit_err = pos - j->limot.histop;
|
||||
info->m = 6;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void sliderGetInfo2 (dxJointSlider *joint, dxJoint::Info2 *info)
|
||||
{
|
||||
int i,s = info->rowskip;
|
||||
int s2=2*s,s3=3*s,s4=4*s;
|
||||
|
||||
// pull out pos and R for both bodies. also get the `connection'
|
||||
// vector pos2-pos1.
|
||||
|
||||
dReal *pos1,*pos2,*R1,*R2;
|
||||
dVector3 c;
|
||||
pos1 = joint->node[0].body->pos;
|
||||
R1 = joint->node[0].body->R;
|
||||
if (joint->node[1].body) {
|
||||
pos2 = joint->node[1].body->pos;
|
||||
R2 = joint->node[1].body->R;
|
||||
for (i=0; i<3; i++) c[i] = pos2[i] - pos1[i];
|
||||
}
|
||||
else {
|
||||
pos2 = 0;
|
||||
R2 = 0;
|
||||
}
|
||||
|
||||
// 3 rows to make body rotations equal
|
||||
info->J1a[0] = 1;
|
||||
info->J1a[s+1] = 1;
|
||||
info->J1a[s2+2] = 1;
|
||||
if (joint->node[1].body) {
|
||||
info->J2a[0] = -1;
|
||||
info->J2a[s+1] = -1;
|
||||
info->J2a[s2+2] = -1;
|
||||
}
|
||||
|
||||
// remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would
|
||||
// result in three equations, so we project along the planespace vectors
|
||||
// so that sliding along the slider axis is disregarded. for symmetry we
|
||||
// also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
|
||||
|
||||
dVector3 ax1; // joint axis in global coordinates (unit length)
|
||||
dVector3 p,q; // plane space of ax1
|
||||
dMULTIPLY0_331 (ax1,R1,joint->axis1);
|
||||
dPlaneSpace (ax1,p,q);
|
||||
if (joint->node[1].body) {
|
||||
dVector3 tmp;
|
||||
dCROSS (tmp, = REAL(0.5) * ,c,p);
|
||||
for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i];
|
||||
for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i];
|
||||
dCROSS (tmp, = REAL(0.5) * ,c,q);
|
||||
for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i];
|
||||
for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i];
|
||||
for (i=0; i<3; i++) info->J2l[s3+i] = -p[i];
|
||||
for (i=0; i<3; i++) info->J2l[s4+i] = -q[i];
|
||||
}
|
||||
for (i=0; i<3; i++) info->J1l[s3+i] = p[i];
|
||||
for (i=0; i<3; i++) info->J1l[s4+i] = q[i];
|
||||
|
||||
// compute the right hand side. the first three elements will result in
|
||||
// relative angular velocity of the two bodies - this is set to bring them
|
||||
// back into alignment. the correcting angular velocity is
|
||||
// |angular_velocity| = angle/time = erp*theta / stepsize
|
||||
// = (erp*fps) * theta
|
||||
// angular_velocity = |angular_velocity| * u
|
||||
// = (erp*fps) * theta * u
|
||||
// where rotation along unit length axis u by theta brings body 2's frame
|
||||
// to qrel with respect to body 1's frame. using a small angle approximation
|
||||
// for sin(), this gives
|
||||
// angular_velocity = (erp*fps) * 2 * v
|
||||
// where the quaternion of the relative rotation between the two bodies is
|
||||
// q = [cos(theta/2) sin(theta/2)*u] = [s v]
|
||||
|
||||
// get qerr = relative rotation (rotation error) between two bodies
|
||||
dQuaternion qerr,e;
|
||||
if (joint->node[1].body) {
|
||||
dQuaternion qq;
|
||||
dQMultiply1 (qq,joint->node[0].body->q,joint->node[1].body->q);
|
||||
dQMultiply2 (qerr,qq,joint->qrel);
|
||||
}
|
||||
else {
|
||||
dQMultiply3 (qerr,joint->node[0].body->q,joint->qrel);
|
||||
}
|
||||
if (qerr[0] < 0) {
|
||||
qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small
|
||||
qerr[2] = -qerr[2];
|
||||
qerr[3] = -qerr[3];
|
||||
}
|
||||
dMULTIPLY0_331 (e,joint->node[0].body->R,qerr+1); // @@@ bad SIMD padding!
|
||||
dReal k = info->fps * info->erp;
|
||||
info->c[0] = 2*k * e[0];
|
||||
info->c[1] = 2*k * e[1];
|
||||
info->c[2] = 2*k * e[2];
|
||||
|
||||
// compute last two elements of right hand side. we want to align the offset
|
||||
// point (in body 2's frame) with the center of body 1.
|
||||
if (joint->node[1].body) {
|
||||
dVector3 ofs; // offset point in global coordinates
|
||||
dMULTIPLY0_331 (ofs,R2,joint->offset);
|
||||
for (i=0; i<3; i++) c[i] += ofs[i];
|
||||
info->c[3] = k * dDOT(p,c);
|
||||
info->c[4] = k * dDOT(q,c);
|
||||
}
|
||||
else {
|
||||
dVector3 ofs; // offset point in global coordinates
|
||||
for (i=0; i<3; i++) ofs[i] = joint->offset[i] - pos1[i];
|
||||
info->c[3] = k * dDOT(p,ofs);
|
||||
info->c[4] = k * dDOT(q,ofs);
|
||||
}
|
||||
|
||||
// if the slider is powered, or has joint limits, add in the extra row
|
||||
joint->limot.addLimot (joint,info,5,ax1,0);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetSliderAxis (dxJointSlider *joint,
|
||||
dReal x, dReal y, dReal z)
|
||||
{
|
||||
int i;
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
|
||||
setAxes (joint,x,y,z,joint->axis1,0);
|
||||
|
||||
// compute initial relative rotation body1 -> body2, or env -> body1
|
||||
// also compute center of body1 w.r.t body 2
|
||||
if (joint->node[1].body) {
|
||||
dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q);
|
||||
dVector3 c;
|
||||
for (i=0; i<3; i++)
|
||||
c[i] = joint->node[0].body->pos[i] - joint->node[1].body->pos[i];
|
||||
dMULTIPLY1_331 (joint->offset,joint->node[1].body->R,c);
|
||||
}
|
||||
else {
|
||||
// set joint->qrel to the transpose of the first body's q
|
||||
joint->qrel[0] = joint->node[0].body->q[0];
|
||||
for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i];
|
||||
for (i=0; i<3; i++) joint->offset[i] = joint->node[0].body->pos[i];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointGetSliderAxis (dxJointSlider *joint, dVector3 result)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(result,"bad result argument");
|
||||
dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
|
||||
getAxis (joint,result,joint->axis1);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetSliderParam (dxJointSlider *joint,
|
||||
int parameter, dReal value)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
|
||||
joint->limot.set (parameter,value);
|
||||
}
|
||||
|
||||
|
||||
extern "C" dReal dJointGetSliderParam (dxJointSlider *joint, int parameter)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
|
||||
return joint->limot.get (parameter);
|
||||
}
|
||||
|
||||
|
||||
dxJoint::Vtable __dslider_vtable = {
|
||||
sizeof(dxJointSlider),
|
||||
(dxJoint::init_fn*) sliderInit,
|
||||
(dxJoint::getInfo1_fn*) sliderGetInfo1,
|
||||
(dxJoint::getInfo2_fn*) sliderGetInfo2,
|
||||
dJointTypeSlider};
|
||||
|
||||
//****************************************************************************
|
||||
// contact
|
||||
|
||||
static void contactInit (dxJointContact *j)
|
||||
{
|
||||
// default frictionless contact. hmmm, this info gets overwritten straight
|
||||
// away anyway, so why bother?
|
||||
j->contact.surface.mode = 0;
|
||||
j->contact.surface.mu = 0;
|
||||
dSetZero (j->contact.geom.pos,4);
|
||||
dSetZero (j->contact.geom.normal,4);
|
||||
j->contact.geom.depth = 0;
|
||||
}
|
||||
|
||||
|
||||
static void contactGetInfo1 (dxJointContact *j, dxJoint::Info1 *info)
|
||||
{
|
||||
// make sure mu's >= 0, then calculate number of constraint rows and number
|
||||
// of unbounded rows.
|
||||
int m = 1, nub=0;
|
||||
if (j->contact.surface.mu < 0) j->contact.surface.mu = 0;
|
||||
if (j->contact.surface.mode & dContactMu2) {
|
||||
if (j->contact.surface.mu > 0) m++;
|
||||
if (j->contact.surface.mu2 < 0) j->contact.surface.mu2 = 0;
|
||||
if (j->contact.surface.mu2 > 0) m++;
|
||||
if (j->contact.surface.mu == dInfinity) nub ++;
|
||||
if (j->contact.surface.mu2 == dInfinity) nub ++;
|
||||
}
|
||||
else {
|
||||
if (j->contact.surface.mu > 0) m += 2;
|
||||
if (j->contact.surface.mu == dInfinity) nub += 2;
|
||||
}
|
||||
|
||||
j->the_m = m;
|
||||
info->m = m;
|
||||
info->nub = nub;
|
||||
}
|
||||
|
||||
|
||||
static void contactGetInfo2 (dxJointContact *j, dxJoint::Info2 *info)
|
||||
{
|
||||
int i,s = info->rowskip;
|
||||
int s2 = 2*s;
|
||||
|
||||
// get normal, with sign adjusted for body1/body2 polarity
|
||||
dVector3 normal;
|
||||
if (j->flags & dJOINT_REVERSE) {
|
||||
normal[0] = j->contact.geom.normal[0];
|
||||
normal[1] = j->contact.geom.normal[1];
|
||||
normal[2] = j->contact.geom.normal[2];
|
||||
}
|
||||
else {
|
||||
normal[0] = - j->contact.geom.normal[0];
|
||||
normal[1] = - j->contact.geom.normal[1];
|
||||
normal[2] = - j->contact.geom.normal[2];
|
||||
}
|
||||
normal[3] = 0; // @@@ hmmm
|
||||
|
||||
// c1,c2 = contact points with respect to body PORs
|
||||
dVector3 c1,c2;
|
||||
for (i=0; i<3; i++) c1[i] = j->contact.geom.pos[i] - j->node[0].body->pos[i];
|
||||
|
||||
// set jacobian for normal
|
||||
info->J1l[0] = normal[0];
|
||||
info->J1l[1] = normal[1];
|
||||
info->J1l[2] = normal[2];
|
||||
dCROSS (info->J1a,=,c1,normal);
|
||||
if (j->node[1].body) {
|
||||
for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] -
|
||||
j->node[1].body->pos[i];
|
||||
info->J2l[0] = -normal[0];
|
||||
info->J2l[1] = -normal[1];
|
||||
info->J2l[2] = -normal[2];
|
||||
dCROSS (info->J2a,= -,c2,normal);
|
||||
}
|
||||
|
||||
// set right hand side and cfm value for normal
|
||||
dReal erp = info->erp;
|
||||
if (j->contact.surface.mode & dContactSoftERP)
|
||||
erp = j->contact.surface.soft_erp;
|
||||
dReal k = info->fps * erp;
|
||||
info->c[0] = k*j->contact.geom.depth;
|
||||
if (j->contact.surface.mode & dContactSoftCFM)
|
||||
info->cfm[0] = j->contact.surface.soft_cfm;
|
||||
|
||||
// deal with bounce
|
||||
if (j->contact.surface.mode & dContactBounce) {
|
||||
// calculate outgoing velocity (-ve for incoming contact)
|
||||
dReal outgoing = dDOT(info->J1l,j->node[0].body->lvel) +
|
||||
dDOT(info->J1a,j->node[0].body->avel);
|
||||
if (j->node[1].body) {
|
||||
outgoing += dDOT(info->J2l,j->node[1].body->lvel) +
|
||||
dDOT(info->J2a,j->node[1].body->avel);
|
||||
}
|
||||
// only apply bounce if the outgoing velocity is greater than the
|
||||
// threshold, and if the resulting c[0] exceeds what we already have.
|
||||
if (j->contact.surface.bounce_vel >= 0 &&
|
||||
(-outgoing) > j->contact.surface.bounce_vel) {
|
||||
dReal newc = - j->contact.surface.bounce * outgoing;
|
||||
if (newc > info->c[0]) info->c[0] = newc;
|
||||
}
|
||||
}
|
||||
|
||||
// set LCP limits for normal
|
||||
info->lo[0] = 0;
|
||||
info->hi[0] = dInfinity;
|
||||
|
||||
// now do jacobian for tangential forces
|
||||
dVector3 t1,t2; // two vectors tangential to normal
|
||||
|
||||
// first friction direction
|
||||
if (j->the_m >= 2) {
|
||||
if (j->contact.surface.mode & dContactFDir1) { // use fdir1 ?
|
||||
t1[0] = j->contact.fdir1[0];
|
||||
t1[1] = j->contact.fdir1[1];
|
||||
t1[2] = j->contact.fdir1[2];
|
||||
dCROSS (t2,=,normal,t1);
|
||||
}
|
||||
else {
|
||||
dPlaneSpace (normal,t1,t2);
|
||||
}
|
||||
info->J1l[s+0] = t1[0];
|
||||
info->J1l[s+1] = t1[1];
|
||||
info->J1l[s+2] = t1[2];
|
||||
dCROSS (info->J1a+s,=,c1,t1);
|
||||
if (j->node[1].body) {
|
||||
info->J2l[s+0] = -t1[0];
|
||||
info->J2l[s+1] = -t1[1];
|
||||
info->J2l[s+2] = -t1[2];
|
||||
dCROSS (info->J2a+s,= -,c2,t1);
|
||||
}
|
||||
// set right hand side
|
||||
if (j->contact.surface.mode & dContactMotion1) {
|
||||
info->c[1] = j->contact.surface.motion1;
|
||||
}
|
||||
// set LCP bounds and friction index. this depends on the approximation
|
||||
// mode
|
||||
info->lo[1] = -j->contact.surface.mu;
|
||||
info->hi[1] = j->contact.surface.mu;
|
||||
if (j->contact.surface.mode & dContactApprox1_1) info->findex[1] = 0;
|
||||
|
||||
// set slip (constraint force mixing)
|
||||
if (j->contact.surface.mode & dContactSlip1)
|
||||
info->cfm[1] = j->contact.surface.slip1;
|
||||
}
|
||||
|
||||
// second friction direction
|
||||
if (j->the_m >= 3) {
|
||||
info->J1l[s2+0] = t2[0];
|
||||
info->J1l[s2+1] = t2[1];
|
||||
info->J1l[s2+2] = t2[2];
|
||||
dCROSS (info->J1a+s2,=,c1,t2);
|
||||
if (j->node[1].body) {
|
||||
info->J2l[s2+0] = -t2[0];
|
||||
info->J2l[s2+1] = -t2[1];
|
||||
info->J2l[s2+2] = -t2[2];
|
||||
dCROSS (info->J2a+s2,= -,c2,t2);
|
||||
}
|
||||
// set right hand side
|
||||
if (j->contact.surface.mode & dContactMotion2) {
|
||||
info->c[2] = j->contact.surface.motion2;
|
||||
}
|
||||
// set LCP bounds and friction index. this depends on the approximation
|
||||
// mode
|
||||
if (j->contact.surface.mode & dContactMu2) {
|
||||
info->lo[2] = -j->contact.surface.mu2;
|
||||
info->hi[2] = j->contact.surface.mu2;
|
||||
}
|
||||
else {
|
||||
info->lo[2] = -j->contact.surface.mu;
|
||||
info->hi[2] = j->contact.surface.mu;
|
||||
}
|
||||
if (j->contact.surface.mode & dContactApprox1_2) info->findex[2] = 0;
|
||||
|
||||
// set slip (constraint force mixing)
|
||||
if (j->contact.surface.mode & dContactSlip2)
|
||||
info->cfm[2] = j->contact.surface.slip2;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
dxJoint::Vtable __dcontact_vtable = {
|
||||
sizeof(dxJointContact),
|
||||
(dxJoint::init_fn*) contactInit,
|
||||
(dxJoint::getInfo1_fn*) contactGetInfo1,
|
||||
(dxJoint::getInfo2_fn*) contactGetInfo2,
|
||||
dJointTypeContact};
|
||||
|
||||
//****************************************************************************
|
||||
// hinge 2. note that this joint must be attached to two bodies for it to work
|
||||
|
||||
static dReal measureHinge2Angle (dxJointHinge2 *joint)
|
||||
{
|
||||
dVector3 a1,a2;
|
||||
dMULTIPLY0_331 (a1,joint->node[1].body->R,joint->axis2);
|
||||
dMULTIPLY1_331 (a2,joint->node[0].body->R,a1);
|
||||
dReal x = dDOT(joint->v1,a2);
|
||||
dReal y = dDOT(joint->v2,a2);
|
||||
return -dAtan2 (y,x);
|
||||
}
|
||||
|
||||
|
||||
static void hinge2Init (dxJointHinge2 *j)
|
||||
{
|
||||
dSetZero (j->anchor1,4);
|
||||
dSetZero (j->anchor2,4);
|
||||
dSetZero (j->axis1,4);
|
||||
j->axis1[0] = 1;
|
||||
dSetZero (j->axis2,4);
|
||||
j->axis2[1] = 1;
|
||||
j->c0 = 0;
|
||||
j->s0 = 0;
|
||||
|
||||
dSetZero (j->v1,4);
|
||||
j->v1[0] = 1;
|
||||
dSetZero (j->v2,4);
|
||||
j->v2[1] = 1;
|
||||
|
||||
j->limot1.init (j->world);
|
||||
j->limot2.init (j->world);
|
||||
|
||||
j->susp_erp = j->world->global_erp;
|
||||
j->susp_cfm = j->world->global_cfm;
|
||||
|
||||
j->flags |= dJOINT_TWOBODIES;
|
||||
}
|
||||
|
||||
|
||||
static void hinge2GetInfo1 (dxJointHinge2 *j, dxJoint::Info1 *info)
|
||||
{
|
||||
info->m = 4;
|
||||
info->nub = 4;
|
||||
|
||||
// see if we're powered or at a joint limit for axis 1
|
||||
int atlimit=0;
|
||||
if ((j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) &&
|
||||
j->limot1.lostop <= j->limot1.histop) {
|
||||
dReal angle = measureHinge2Angle (j);
|
||||
if (j->limot1.testRotationalLimit (angle)) atlimit = 1;
|
||||
}
|
||||
if (atlimit || j->limot1.fmax > 0) info->m++;
|
||||
|
||||
// see if we're powering axis 2 (we currently never limit this axis)
|
||||
j->limot2.limit = 0;
|
||||
if (j->limot2.fmax > 0) info->m++;
|
||||
}
|
||||
|
||||
|
||||
// macro that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are
|
||||
// relative to body 1 and 2 initially) and then computes the constrained
|
||||
// rotational axis as the cross product of ax1 and ax2.
|
||||
// the sin and cos of the angle between axis 1 and 2 is computed, this comes
|
||||
// from dot and cross product rules.
|
||||
|
||||
#define HINGE2_GET_AXIS_INFO(axis,sin_angle,cos_angle) \
|
||||
dVector3 ax1,ax2; \
|
||||
dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); \
|
||||
dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); \
|
||||
dCROSS (axis,=,ax1,ax2); \
|
||||
sin_angle = dSqrt (axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); \
|
||||
cos_angle = dDOT (ax1,ax2);
|
||||
|
||||
|
||||
static void hinge2GetInfo2 (dxJointHinge2 *joint, dxJoint::Info2 *info)
|
||||
{
|
||||
// get information we need to set the hinge row
|
||||
dReal s,c;
|
||||
dVector3 q;
|
||||
HINGE2_GET_AXIS_INFO (q,s,c);
|
||||
dNormalize3 (q); // @@@ quicker: divide q by s ?
|
||||
|
||||
// set the three ball-and-socket rows (aligned to the suspension axis ax1)
|
||||
setBall2 (joint,info,joint->anchor1,joint->anchor2,ax1,joint->susp_erp);
|
||||
|
||||
// set the hinge row
|
||||
int s3=3*info->rowskip;
|
||||
info->J1a[s3+0] = q[0];
|
||||
info->J1a[s3+1] = q[1];
|
||||
info->J1a[s3+2] = q[2];
|
||||
if (joint->node[1].body) {
|
||||
info->J2a[s3+0] = -q[0];
|
||||
info->J2a[s3+1] = -q[1];
|
||||
info->J2a[s3+2] = -q[2];
|
||||
}
|
||||
|
||||
// compute the right hand side for the constrained rotational DOF.
|
||||
// axis 1 and axis 2 are separated by an angle `theta'. the desired
|
||||
// separation angle is theta0. sin(theta0) and cos(theta0) are recorded
|
||||
// in the joint structure. the correcting angular velocity is:
|
||||
// |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize
|
||||
// = (erp*fps) * (theta0-theta)
|
||||
// (theta0-theta) can be computed using the following small-angle-difference
|
||||
// approximation:
|
||||
// theta0-theta ~= tan(theta0-theta)
|
||||
// = sin(theta0-theta)/cos(theta0-theta)
|
||||
// = (c*s0 - s*c0) / (c*c0 + s*s0)
|
||||
// = c*s0 - s*c0 assuming c*c0 + s*s0 ~= 1
|
||||
// where c = cos(theta), s = sin(theta)
|
||||
// c0 = cos(theta0), s0 = sin(theta0)
|
||||
|
||||
dReal k = info->fps * info->erp;
|
||||
info->c[3] = k * (joint->c0 * s - joint->s0 * c);
|
||||
|
||||
// if the axis1 hinge is powered, or has joint limits, add in more stuff
|
||||
int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1);
|
||||
|
||||
// if the axis2 hinge is powered, add in more stuff
|
||||
joint->limot2.addLimot (joint,info,row,ax2,1);
|
||||
|
||||
// set parameter for the suspension
|
||||
info->cfm[0] = joint->susp_cfm;
|
||||
}
|
||||
|
||||
|
||||
// compute vectors v1 and v2 (embedded in body1), used to measure angle
|
||||
// between body 1 and body 2
|
||||
|
||||
static void makeHinge2V1andV2 (dxJointHinge2 *joint)
|
||||
{
|
||||
if (joint->node[0].body) {
|
||||
// get axis 1 and 2 in global coords
|
||||
dVector3 ax1,ax2,v;
|
||||
dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
|
||||
dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2);
|
||||
|
||||
// don't do anything if the axis1 or axis2 vectors are zero or the same
|
||||
if ((ax1[0]==0 && ax1[1]==0 && ax1[2]==0) ||
|
||||
(ax2[0]==0 && ax2[1]==0 && ax2[2]==0) ||
|
||||
(ax1[0]==ax2[0] && ax1[1]==ax2[1] && ax1[2]==ax2[2])) return;
|
||||
|
||||
// modify axis 2 so it's perpendicular to axis 1
|
||||
dReal k = dDOT(ax1,ax2);
|
||||
for (int i=0; i<3; i++) ax2[i] -= k*ax1[i];
|
||||
dNormalize3 (ax2);
|
||||
|
||||
// make v1 = modified axis2, v2 = axis1 x (modified axis2)
|
||||
dCROSS (v,=,ax1,ax2);
|
||||
dMULTIPLY1_331 (joint->v1,joint->node[0].body->R,ax2);
|
||||
dMULTIPLY1_331 (joint->v2,joint->node[0].body->R,v);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetHinge2Anchor (dxJointHinge2 *joint,
|
||||
dReal x, dReal y, dReal z)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
|
||||
setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
|
||||
makeHinge2V1andV2 (joint);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetHinge2Axis1 (dxJointHinge2 *joint,
|
||||
dReal x, dReal y, dReal z)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
|
||||
if (joint->node[0].body) {
|
||||
dReal q[4];
|
||||
q[0] = x;
|
||||
q[1] = y;
|
||||
q[2] = z;
|
||||
q[3] = 0;
|
||||
dNormalize3 (q);
|
||||
dMULTIPLY1_331 (joint->axis1,joint->node[0].body->R,q);
|
||||
joint->axis1[3] = 0;
|
||||
|
||||
// compute the sin and cos of the angle between axis 1 and axis 2
|
||||
dVector3 ax;
|
||||
HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0);
|
||||
}
|
||||
makeHinge2V1andV2 (joint);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetHinge2Axis2 (dxJointHinge2 *joint,
|
||||
dReal x, dReal y, dReal z)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
|
||||
if (joint->node[1].body) {
|
||||
dReal q[4];
|
||||
q[0] = x;
|
||||
q[1] = y;
|
||||
q[2] = z;
|
||||
q[3] = 0;
|
||||
dNormalize3 (q);
|
||||
dMULTIPLY1_331 (joint->axis2,joint->node[1].body->R,q);
|
||||
joint->axis1[3] = 0;
|
||||
|
||||
// compute the sin and cos of the angle between axis 1 and axis 2
|
||||
dVector3 ax;
|
||||
HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0);
|
||||
}
|
||||
makeHinge2V1andV2 (joint);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetHinge2Param (dxJointHinge2 *joint,
|
||||
int parameter, dReal value)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
|
||||
if ((parameter & 0xff00) == 0x100) {
|
||||
joint->limot2.set (parameter & 0xff,value);
|
||||
}
|
||||
else {
|
||||
if (parameter == dParamSuspensionERP) joint->susp_erp = value;
|
||||
else if (parameter == dParamSuspensionCFM) joint->susp_cfm = value;
|
||||
else joint->limot1.set (parameter,value);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointGetHinge2Anchor (dxJointHinge2 *joint, dVector3 result)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(result,"bad result argument");
|
||||
dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
|
||||
getAnchor (joint,result,joint->anchor1);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointGetHinge2Axis1 (dxJointHinge2 *joint, dVector3 result)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(result,"bad result argument");
|
||||
dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
|
||||
if (joint->node[0].body) {
|
||||
dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointGetHinge2Axis2 (dxJointHinge2 *joint, dVector3 result)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(result,"bad result argument");
|
||||
dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
|
||||
if (joint->node[1].body) {
|
||||
dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis2);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
extern "C" dReal dJointGetHinge2Param (dxJointHinge2 *joint, int parameter)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
|
||||
if ((parameter & 0xff00) == 0x100) {
|
||||
return joint->limot2.get (parameter & 0xff);
|
||||
}
|
||||
else {
|
||||
if (parameter == dParamSuspensionERP) return joint->susp_erp;
|
||||
else if (parameter == dParamSuspensionCFM) return joint->susp_cfm;
|
||||
else return joint->limot1.get (parameter);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
extern "C" dReal dJointGetHinge2Angle1 (dxJointHinge2 *joint)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
|
||||
if (joint->node[0].body) return measureHinge2Angle (joint);
|
||||
else return 0;
|
||||
}
|
||||
|
||||
|
||||
extern "C" dReal dJointGetHinge2Angle1Rate (dxJointHinge2 *joint)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
|
||||
if (joint->node[0].body) {
|
||||
dVector3 axis;
|
||||
dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1);
|
||||
dReal rate = dDOT(axis,joint->node[0].body->avel);
|
||||
if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel);
|
||||
return rate;
|
||||
}
|
||||
else return 0;
|
||||
}
|
||||
|
||||
|
||||
extern "C" dReal dJointGetHinge2Angle2Rate (dxJointHinge2 *joint)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
|
||||
if (joint->node[0].body && joint->node[1].body) {
|
||||
dVector3 axis;
|
||||
dMULTIPLY0_331 (axis,joint->node[1].body->R,joint->axis2);
|
||||
dReal rate = dDOT(axis,joint->node[0].body->avel);
|
||||
if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel);
|
||||
return rate;
|
||||
}
|
||||
else return 0;
|
||||
}
|
||||
|
||||
|
||||
dxJoint::Vtable __dhinge2_vtable = {
|
||||
sizeof(dxJointHinge2),
|
||||
(dxJoint::init_fn*) hinge2Init,
|
||||
(dxJoint::getInfo1_fn*) hinge2GetInfo1,
|
||||
(dxJoint::getInfo2_fn*) hinge2GetInfo2,
|
||||
dJointTypeHinge2};
|
||||
|
||||
//****************************************************************************
|
||||
// universal
|
||||
|
||||
static void universalInit (dxJointUniversal *j)
|
||||
{
|
||||
dSetZero (j->anchor1,4);
|
||||
dSetZero (j->anchor2,4);
|
||||
dSetZero (j->axis1,4);
|
||||
j->axis1[0] = 1;
|
||||
dSetZero (j->axis2,4);
|
||||
j->axis2[1] = 1;
|
||||
}
|
||||
|
||||
|
||||
static void universalGetInfo1 (dxJointUniversal *j, dxJoint::Info1 *info)
|
||||
{
|
||||
info->nub = 4;
|
||||
info->m = 4;
|
||||
}
|
||||
|
||||
|
||||
static void universalGetInfo2 (dxJointUniversal *joint, dxJoint::Info2 *info)
|
||||
{
|
||||
// set the three ball-and-socket rows
|
||||
setBall (joint,info,joint->anchor1,joint->anchor2);
|
||||
|
||||
// set the universal joint row. the angular velocity about an axis
|
||||
// perpendicular to both joint axes should be equal. thus the constraint
|
||||
// equation is
|
||||
// p*w1 - p*w2 = 0
|
||||
// where p is a vector normal to both joint axes, and w1 and w2
|
||||
// are the angular velocity vectors of the two bodies.
|
||||
|
||||
// length 1 joint axis in global coordinates, from each body
|
||||
dVector3 ax1, ax2;
|
||||
// length 1 vector perpendicular to ax1 and ax2. Neither body can rotate
|
||||
// about this.
|
||||
dVector3 p;
|
||||
|
||||
// This says "ax1 = joint->node[0].body->R * joint->axis1"
|
||||
dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
|
||||
if (joint->node[1].body) {
|
||||
dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2);
|
||||
}
|
||||
else {
|
||||
ax2[0] = joint->axis2[0];
|
||||
ax2[1] = joint->axis2[1];
|
||||
ax2[2] = joint->axis2[2];
|
||||
}
|
||||
|
||||
// if ax1 and ax2 are almost parallel, p won't be perpendicular to them.
|
||||
// Is there some more robust way to do this?
|
||||
dCROSS(p, =, ax1, ax2);
|
||||
dNormalize3(p);
|
||||
|
||||
int s3=3*info->rowskip;
|
||||
|
||||
info->J1a[s3+0] = p[0];
|
||||
info->J1a[s3+1] = p[1];
|
||||
info->J1a[s3+2] = p[2];
|
||||
|
||||
if (joint->node[1].body) {
|
||||
info->J2a[s3+0] = -p[0];
|
||||
info->J2a[s3+1] = -p[1];
|
||||
info->J2a[s3+2] = -p[2];
|
||||
}
|
||||
|
||||
// compute the right hand side of the constraint equation. set relative
|
||||
// body velocities along p to bring the axes back to perpendicular.
|
||||
// If ax1, ax2 are unit length joint axes as computed from body1 and
|
||||
// body2, we need to rotate both bodies along the axis p. If theta
|
||||
// is the angle between ax1 and ax2, we need an angular velocity
|
||||
// along p to cover the angle erp * (theta - Pi/2) in one step:
|
||||
//
|
||||
// |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize
|
||||
// = (erp*fps) * (theta - Pi/2)
|
||||
//
|
||||
// if theta is close to Pi/2,
|
||||
// theta - Pi/2 ~= cos(theta), so
|
||||
// |angular_velocity| = (erp*fps) * (ax1 dot ax2)
|
||||
|
||||
info->c[3] = info->fps * info->erp * - dDOT(ax1, ax2);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetUniversalAnchor (dxJointUniversal *joint,
|
||||
dReal x, dReal y, dReal z)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
|
||||
setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetUniversalAxis1 (dxJointUniversal *joint,
|
||||
dReal x, dReal y, dReal z)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
|
||||
if (joint->node[0].body) {
|
||||
dReal q[4];
|
||||
q[0] = x;
|
||||
q[1] = y;
|
||||
q[2] = z;
|
||||
q[3] = 0;
|
||||
dNormalize3 (q);
|
||||
dMULTIPLY1_331 (joint->axis1,joint->node[0].body->R,q);
|
||||
}
|
||||
joint->axis1[3] = 0;
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetUniversalAxis2 (dxJointUniversal *joint,
|
||||
dReal x, dReal y, dReal z)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
|
||||
if (joint->node[1].body) {
|
||||
dReal q[4];
|
||||
q[0] = x;
|
||||
q[1] = y;
|
||||
q[2] = z;
|
||||
q[3] = 0;
|
||||
dNormalize3 (q);
|
||||
dMULTIPLY1_331 (joint->axis2,joint->node[1].body->R,q);
|
||||
}
|
||||
joint->axis2[3] = 0;
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointGetUniversalAnchor (dxJointUniversal *joint,
|
||||
dVector3 result)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(result,"bad result argument");
|
||||
dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
|
||||
getAnchor (joint,result,joint->anchor1);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointGetUniversalAxis1 (dxJointUniversal *joint,
|
||||
dVector3 result)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(result,"bad result argument");
|
||||
dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
|
||||
if (joint->node[0].body) {
|
||||
dMULTIPLY0_331 (result, joint->node[0].body->R, joint->axis1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointGetUniversalAxis2 (dxJointUniversal *joint,
|
||||
dVector3 result)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(result,"bad result argument");
|
||||
dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
|
||||
if (joint->node[1].body) {
|
||||
dMULTIPLY0_331 (result, joint->node[1].body->R, joint->axis2);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
dxJoint::Vtable __duniversal_vtable = {
|
||||
sizeof(dxJointUniversal),
|
||||
(dxJoint::init_fn*) universalInit,
|
||||
(dxJoint::getInfo1_fn*) universalGetInfo1,
|
||||
(dxJoint::getInfo2_fn*) universalGetInfo2,
|
||||
dJointTypeUniversal};
|
||||
|
||||
//****************************************************************************
|
||||
// angular motor
|
||||
|
||||
static void amotorInit (dxJointAMotor *j)
|
||||
{
|
||||
int i;
|
||||
j->num = 0;
|
||||
j->mode = dAMotorUser;
|
||||
for (i=0; i<3; i++) {
|
||||
j->rel[i] = 0;
|
||||
dSetZero (j->axis[i],4);
|
||||
j->limot[i].init (j->world);
|
||||
j->angle[i] = 0;
|
||||
}
|
||||
dSetZero (j->reference1,4);
|
||||
dSetZero (j->reference2,4);
|
||||
|
||||
j->flags |= dJOINT_TWOBODIES;
|
||||
}
|
||||
|
||||
|
||||
// compute the 3 axes in global coordinates
|
||||
|
||||
static void amotorComputeGlobalAxes (dxJointAMotor *joint, dVector3 ax[3])
|
||||
{
|
||||
if (joint->mode == dAMotorEuler) {
|
||||
// special handling for euler mode
|
||||
dMULTIPLY0_331 (ax[0],joint->node[0].body->R,joint->axis[0]);
|
||||
dMULTIPLY0_331 (ax[2],joint->node[1].body->R,joint->axis[2]);
|
||||
dCROSS (ax[1],=,ax[2],ax[0]);
|
||||
dNormalize3 (ax[1]);
|
||||
}
|
||||
else {
|
||||
for (int i=0; i < joint->num; i++) {
|
||||
if (joint->rel[i] == 1) {
|
||||
// relative to b1
|
||||
dMULTIPLY0_331 (ax[i],joint->node[0].body->R,joint->axis[i]);
|
||||
}
|
||||
if (joint->rel[i] == 2) {
|
||||
// relative to b2
|
||||
dMULTIPLY0_331 (ax[i],joint->node[1].body->R,joint->axis[i]);
|
||||
}
|
||||
else {
|
||||
// global - just copy it
|
||||
ax[i][0] = joint->axis[i][0];
|
||||
ax[i][1] = joint->axis[i][1];
|
||||
ax[i][2] = joint->axis[i][2];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void amotorComputeEulerAngles (dxJointAMotor *joint, dVector3 ax[3])
|
||||
{
|
||||
// assumptions:
|
||||
// global axes already calculated --> ax
|
||||
// axis[0] is relative to body 1 --> global ax[0]
|
||||
// axis[2] is relative to body 2 --> global ax[2]
|
||||
// ax[1] = ax[2] x ax[0]
|
||||
// original ax[0] and ax[2] are perpendicular
|
||||
// reference1 is perpendicular to ax[0] (in body 1 frame)
|
||||
// reference2 is perpendicular to ax[2] (in body 2 frame)
|
||||
// all ax[] and reference vectors are unit length
|
||||
|
||||
// calculate references in global frame
|
||||
dVector3 ref1,ref2;
|
||||
dMULTIPLY0_331 (ref1,joint->node[0].body->R,joint->reference1);
|
||||
dMULTIPLY0_331 (ref2,joint->node[1].body->R,joint->reference2);
|
||||
|
||||
// get q perpendicular to both ax[0] and ref1, get first euler angle
|
||||
dVector3 q;
|
||||
dCROSS (q,=,ax[0],ref1);
|
||||
joint->angle[0] = -dAtan2 (dDOT(ax[2],q),dDOT(ax[2],ref1));
|
||||
|
||||
// get q perpendicular to both ax[0] and ax[1], get second euler angle
|
||||
dCROSS (q,=,ax[0],ax[1]);
|
||||
joint->angle[1] = -dAtan2 (dDOT(ax[2],ax[0]),dDOT(ax[2],q));
|
||||
|
||||
// get q perpendicular to both ax[1] and ax[2], get third euler angle
|
||||
dCROSS (q,=,ax[1],ax[2]);
|
||||
joint->angle[2] = -dAtan2 (dDOT(ref2,ax[1]), dDOT(ref2,q));
|
||||
}
|
||||
|
||||
|
||||
// set the reference vectors as follows:
|
||||
// * reference1 = current axis[2] relative to body 1
|
||||
// * reference2 = current axis[0] relative to body 2
|
||||
// this assumes that:
|
||||
// * axis[0] is relative to body 1
|
||||
// * axis[2] is relative to body 2
|
||||
|
||||
static void amotorSetEulerReferenceVectors (dxJointAMotor *j)
|
||||
{
|
||||
if (j->node[0].body && j->node[1].body) {
|
||||
dVector3 r; // axis[2] and axis[0] in global coordinates
|
||||
dMULTIPLY0_331 (r,j->node[1].body->R,j->axis[2]);
|
||||
dMULTIPLY1_331 (j->reference1,j->node[0].body->R,r);
|
||||
dMULTIPLY0_331 (r,j->node[0].body->R,j->axis[0]);
|
||||
dMULTIPLY1_331 (j->reference2,j->node[1].body->R,r);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void amotorGetInfo1 (dxJointAMotor *j, dxJoint::Info1 *info)
|
||||
{
|
||||
info->m = 0;
|
||||
info->nub = 0;
|
||||
|
||||
// compute the axes and angles, if in euler mode
|
||||
if (j->mode == dAMotorEuler) {
|
||||
dVector3 ax[3];
|
||||
amotorComputeGlobalAxes (j,ax);
|
||||
amotorComputeEulerAngles (j,ax);
|
||||
}
|
||||
|
||||
// see if we're powered or at a joint limit for each axis
|
||||
for (int i=0; i < j->num; i++) {
|
||||
if (j->limot[i].testRotationalLimit (j->angle[i]) ||
|
||||
j->limot[i].fmax > 0) {
|
||||
info->m++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void amotorGetInfo2 (dxJointAMotor *joint, dxJoint::Info2 *info)
|
||||
{
|
||||
int i;
|
||||
|
||||
// compute the axes (if not global)
|
||||
dVector3 ax[3];
|
||||
amotorComputeGlobalAxes (joint,ax);
|
||||
|
||||
// in euler angle mode we do not actually constrain the angular velocity
|
||||
// along the axes axis[0] and axis[2] (although we do use axis[1]) :
|
||||
//
|
||||
// to get constrain w2-w1 along ...not
|
||||
// ------ --------------------- ------
|
||||
// d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
|
||||
// d(angle[1])/dt = 0 ax[1]
|
||||
// d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
|
||||
//
|
||||
// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
|
||||
// to prove the result for angle[0], write the expression for angle[0] from
|
||||
// GetInfo1 then take the derivative. to prove this for angle[2] it is
|
||||
// easier to take the euler rate expression for d(angle[2])/dt with respect
|
||||
// to the components of w and set that to 0.
|
||||
|
||||
dVector3 *axptr[3];
|
||||
axptr[0] = &ax[0];
|
||||
axptr[1] = &ax[1];
|
||||
axptr[2] = &ax[2];
|
||||
|
||||
dVector3 ax0_cross_ax1;
|
||||
dVector3 ax1_cross_ax2;
|
||||
if (joint->mode == dAMotorEuler) {
|
||||
dCROSS (ax0_cross_ax1,=,ax[0],ax[1]);
|
||||
axptr[2] = &ax0_cross_ax1;
|
||||
dCROSS (ax1_cross_ax2,=,ax[1],ax[2]);
|
||||
axptr[0] = &ax1_cross_ax2;
|
||||
}
|
||||
|
||||
int row=0;
|
||||
for (i=0; i < joint->num; i++) {
|
||||
row += joint->limot[i].addLimot (joint,info,row,*(axptr[i]),1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetAMotorNumAxes (dxJointAMotor *joint, int num)
|
||||
{
|
||||
dAASSERT(joint && num >= 0 && num <= 3);
|
||||
dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
|
||||
if (joint->mode == dAMotorEuler) {
|
||||
joint->num = 3;
|
||||
}
|
||||
else {
|
||||
if (num < 0) num = 0;
|
||||
if (num > 3) num = 3;
|
||||
joint->num = num;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetAMotorAxis (dxJointAMotor *joint, int anum, int rel,
|
||||
dReal x, dReal y, dReal z)
|
||||
{
|
||||
dAASSERT(joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2);
|
||||
dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
|
||||
if (anum < 0) anum = 0;
|
||||
if (anum > 2) anum = 2;
|
||||
joint->rel[anum] = rel;
|
||||
|
||||
// x,y,z is always in global coordinates regardless of rel, so we may have
|
||||
// to convert it to be relative to a body
|
||||
dVector3 r;
|
||||
r[0] = x;
|
||||
r[1] = y;
|
||||
r[2] = z;
|
||||
r[3] = 0;
|
||||
if (rel > 0) {
|
||||
if (rel==1) {
|
||||
dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->R,r);
|
||||
}
|
||||
else {
|
||||
dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->R,r);
|
||||
}
|
||||
}
|
||||
else {
|
||||
joint->axis[anum][0] = r[0];
|
||||
joint->axis[anum][1] = r[1];
|
||||
joint->axis[anum][2] = r[2];
|
||||
}
|
||||
dNormalize3 (joint->axis[anum]);
|
||||
if (joint->mode == dAMotorEuler) amotorSetEulerReferenceVectors (joint);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetAMotorAngle (dxJointAMotor *joint, int anum,
|
||||
dReal angle)
|
||||
{
|
||||
dAASSERT(joint && anum >= 0 && anum < 3);
|
||||
dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
|
||||
if (joint->mode == dAMotorUser) {
|
||||
if (anum < 0) anum = 0;
|
||||
if (anum > 3) anum = 3;
|
||||
joint->angle[anum] = angle;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetAMotorParam (dxJointAMotor *joint, int parameter,
|
||||
dReal value)
|
||||
{
|
||||
dAASSERT(joint);
|
||||
dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
|
||||
int anum = parameter >> 8;
|
||||
if (anum < 0) anum = 0;
|
||||
if (anum > 2) anum = 2;
|
||||
parameter &= 0xff;
|
||||
joint->limot[anum].set (parameter, value);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetAMotorMode (dxJointAMotor *joint, int mode)
|
||||
{
|
||||
dAASSERT(joint);
|
||||
dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
|
||||
joint->mode = mode;
|
||||
if (joint->mode == dAMotorEuler) {
|
||||
joint->num = 3;
|
||||
amotorSetEulerReferenceVectors (joint);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
extern "C" int dJointGetAMotorNumAxes (dxJointAMotor *joint)
|
||||
{
|
||||
dAASSERT(joint);
|
||||
dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
|
||||
return joint->num;
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointGetAMotorAxis (dxJointAMotor *joint, int anum,
|
||||
dVector3 result)
|
||||
{
|
||||
dAASSERT(joint && anum >= 0 && anum < 3);
|
||||
dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
|
||||
if (anum < 0) anum = 0;
|
||||
if (anum > 2) anum = 2;
|
||||
if (joint->rel[anum] > 0) {
|
||||
if (joint->rel[anum]==1) {
|
||||
dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis[anum]);
|
||||
}
|
||||
else {
|
||||
dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis[anum]);
|
||||
}
|
||||
}
|
||||
else {
|
||||
result[0] = joint->axis[anum][0];
|
||||
result[1] = joint->axis[anum][1];
|
||||
result[2] = joint->axis[anum][2];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
extern "C" int dJointGetAMotorAxisRel (dxJointAMotor *joint, int anum)
|
||||
{
|
||||
dAASSERT(joint && anum >= 0 && anum < 3);
|
||||
dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
|
||||
if (anum < 0) anum = 0;
|
||||
if (anum > 2) anum = 2;
|
||||
return joint->rel[anum];
|
||||
}
|
||||
|
||||
|
||||
extern "C" dReal dJointGetAMotorAngle (dxJointAMotor *joint, int anum)
|
||||
{
|
||||
dAASSERT(joint && anum >= 0 && anum < 3);
|
||||
dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
|
||||
if (anum < 0) anum = 0;
|
||||
if (anum > 3) anum = 3;
|
||||
return joint->angle[anum];
|
||||
}
|
||||
|
||||
|
||||
extern "C" dReal dJointGetAMotorAngleRate (dxJointAMotor *joint, int anum)
|
||||
{
|
||||
// @@@
|
||||
dDebug (0,"not yet implemented");
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
extern "C" dReal dJointGetAMotorParam (dxJointAMotor *joint, int parameter)
|
||||
{
|
||||
dAASSERT(joint);
|
||||
dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
|
||||
int anum = parameter >> 8;
|
||||
if (anum < 0) anum = 0;
|
||||
if (anum > 2) anum = 2;
|
||||
parameter &= 0xff;
|
||||
return joint->limot[anum].get (parameter);
|
||||
}
|
||||
|
||||
|
||||
extern "C" int dJointGetAMotorMode (dxJointAMotor *joint)
|
||||
{
|
||||
dAASSERT(joint);
|
||||
dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
|
||||
return joint->mode;
|
||||
}
|
||||
|
||||
|
||||
dxJoint::Vtable __damotor_vtable = {
|
||||
sizeof(dxJointAMotor),
|
||||
(dxJoint::init_fn*) amotorInit,
|
||||
(dxJoint::getInfo1_fn*) amotorGetInfo1,
|
||||
(dxJoint::getInfo2_fn*) amotorGetInfo2,
|
||||
dJointTypeAMotor};
|
||||
|
||||
//****************************************************************************
|
||||
// fixed joint
|
||||
|
||||
static void fixedInit (dxJointFixed *j)
|
||||
{
|
||||
dSetZero (j->offset,4);
|
||||
}
|
||||
|
||||
|
||||
static void fixedGetInfo1 (dxJointFixed *j, dxJoint::Info1 *info)
|
||||
{
|
||||
info->m = 6;
|
||||
info->nub = 6;
|
||||
}
|
||||
|
||||
|
||||
static void fixedGetInfo2 (dxJointFixed *joint, dxJoint::Info2 *info)
|
||||
{
|
||||
int s = info->rowskip;
|
||||
|
||||
// set jacobian
|
||||
info->J1l[0] = 1;
|
||||
info->J1l[s+1] = 1;
|
||||
info->J1l[2*s+2] = 1;
|
||||
info->J1a[3*s] = 1;
|
||||
info->J1a[4*s+1] = 1;
|
||||
info->J1a[5*s+2] = 1;
|
||||
|
||||
dVector3 ofs;
|
||||
if (joint->node[1].body) {
|
||||
dMULTIPLY0_331 (ofs,joint->node[0].body->R,joint->offset);
|
||||
dCROSSMAT (info->J1a,ofs,s,+,-);
|
||||
info->J2l[0] = -1;
|
||||
info->J2l[s+1] = -1;
|
||||
info->J2l[2*s+2] = -1;
|
||||
info->J2a[3*s] = -1;
|
||||
info->J2a[4*s+1] = -1;
|
||||
info->J2a[5*s+2] = -1;
|
||||
}
|
||||
|
||||
// set right hand side for the first three rows (linear)
|
||||
dReal k = info->fps * info->erp;
|
||||
if (joint->node[1].body) {
|
||||
for (int j=0; j<3; j++)
|
||||
info->c[j] = k * (joint->node[1].body->pos[j] -
|
||||
joint->node[0].body->pos[j] + ofs[j]);
|
||||
}
|
||||
else {
|
||||
for (int j=0; j<3; j++)
|
||||
info->c[j] = k * (joint->offset[j] - joint->node[0].body->pos[j]);
|
||||
}
|
||||
|
||||
// set right hand side for the next three rows (angular). this code is
|
||||
// borrowed from the slider, so look at the comments there.
|
||||
// @@@ make a function common to both the slider and this joint !!!
|
||||
|
||||
// get qerr = relative rotation (rotation error) between two bodies
|
||||
dQuaternion qerr,e;
|
||||
if (joint->node[1].body) {
|
||||
dQMultiply1 (qerr,joint->node[0].body->q,joint->node[1].body->q);
|
||||
}
|
||||
else {
|
||||
qerr[0] = joint->node[0].body->q[0];
|
||||
for (int i=1; i<4; i++) qerr[i] = -joint->node[0].body->q[i];
|
||||
}
|
||||
if (qerr[0] < 0) {
|
||||
qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small
|
||||
qerr[2] = -qerr[2];
|
||||
qerr[3] = -qerr[3];
|
||||
}
|
||||
dMULTIPLY0_331 (e,joint->node[0].body->R,qerr+1); // @@@ bad SIMD padding!
|
||||
info->c[3] = 2*k * e[0];
|
||||
info->c[4] = 2*k * e[1];
|
||||
info->c[5] = 2*k * e[2];
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dJointSetFixed (dxJointFixed *joint)
|
||||
{
|
||||
dUASSERT(joint,"bad joint argument");
|
||||
dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not fixed");
|
||||
int i;
|
||||
|
||||
// compute the offset between the bodies
|
||||
if (joint->node[0].body) {
|
||||
if (joint->node[1].body) {
|
||||
dReal ofs[4];
|
||||
for (i=0; i<4; i++) ofs[i] = joint->node[0].body->pos[i];
|
||||
for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->pos[i];
|
||||
dMULTIPLY1_331 (joint->offset,joint->node[0].body->R,ofs);
|
||||
}
|
||||
else {
|
||||
for (i=0; i<4; i++) joint->offset[i] = joint->node[0].body->pos[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
dxJoint::Vtable __dfixed_vtable = {
|
||||
sizeof(dxJointFixed),
|
||||
(dxJoint::init_fn*) fixedInit,
|
||||
(dxJoint::getInfo1_fn*) fixedGetInfo1,
|
||||
(dxJoint::getInfo2_fn*) fixedGetInfo2,
|
||||
dJointTypeFixed};
|
||||
|
||||
//****************************************************************************
|
||||
// null joint
|
||||
|
||||
static void nullGetInfo1 (dxJointNull *j, dxJoint::Info1 *info)
|
||||
{
|
||||
info->m = 0;
|
||||
info->nub = 0;
|
||||
}
|
||||
|
||||
|
||||
static void nullGetInfo2 (dxJointNull *joint, dxJoint::Info2 *info)
|
||||
{
|
||||
dDebug (0,"this should never get called");
|
||||
}
|
||||
|
||||
|
||||
dxJoint::Vtable __dnull_vtable = {
|
||||
sizeof(dxJointNull),
|
||||
(dxJoint::init_fn*) 0,
|
||||
(dxJoint::getInfo1_fn*) nullGetInfo1,
|
||||
(dxJoint::getInfo2_fn*) nullGetInfo2,
|
||||
dJointTypeNull};
|
||||
260
extern/ode/dist/ode/src/joint.h
vendored
260
extern/ode/dist/ode/src/joint.h
vendored
@@ -1,260 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef _ODE_JOINT_H_
|
||||
#define _ODE_JOINT_H_
|
||||
|
||||
|
||||
#include "objects.h"
|
||||
#include <ode/contact.h>
|
||||
#include "obstack.h"
|
||||
|
||||
|
||||
// joint flags
|
||||
enum {
|
||||
// if this flag is set, the joint was allocated in a joint group
|
||||
dJOINT_INGROUP = 1,
|
||||
|
||||
// if this flag is set, the joint was attached with arguments (0,body).
|
||||
// our convention is to treat all attaches as (body,0), i.e. so node[0].body
|
||||
// is always nonzero, so this flag records the fact that the arguments were
|
||||
// swapped.
|
||||
dJOINT_REVERSE = 2,
|
||||
|
||||
// if this flag is set, the joint can not have just one body attached to it,
|
||||
// it must have either zero or two bodies attached.
|
||||
dJOINT_TWOBODIES = 4
|
||||
};
|
||||
|
||||
|
||||
// there are two of these nodes in the joint, one for each connection to a
|
||||
// body. these are node of a linked list kept by each body of it's connecting
|
||||
// joints. but note that the body pointer in each node points to the body that
|
||||
// makes use of the *other* node, not this node. this trick makes it a bit
|
||||
// easier to traverse the body/joint graph.
|
||||
|
||||
struct dxJointNode {
|
||||
dxJoint *joint; // pointer to enclosing dxJoint object
|
||||
dxBody *body; // *other* body this joint is connected to
|
||||
dxJointNode *next; // next node in body's list of connected joints
|
||||
};
|
||||
|
||||
|
||||
struct dxJoint : public dObject {
|
||||
// naming convention: the "first" body this is connected to is node[0].body,
|
||||
// and the "second" body is node[1].body. if this joint is only connected
|
||||
// to one body then the second body is 0.
|
||||
|
||||
// info returned by getInfo1 function. the constraint dimension is m (<=6).
|
||||
// i.e. that is the total number of rows in the jacobian. `nub' is the
|
||||
// number of unbounded variables (which have lo,hi = -/+ infinity).
|
||||
|
||||
struct Info1 {
|
||||
int m,nub;
|
||||
};
|
||||
|
||||
// info returned by getInfo2 function
|
||||
|
||||
struct Info2 {
|
||||
// integrator parameters: frames per second (1/stepsize), default error
|
||||
// reduction parameter (0..1).
|
||||
dReal fps,erp;
|
||||
|
||||
// for the first and second body, pointers to two (linear and angular)
|
||||
// n*3 jacobian sub matrices, stored by rows. these matrices will have
|
||||
// been initialized to 0 on entry. if the second body is zero then the
|
||||
// J2xx pointers may be 0.
|
||||
dReal *J1l,*J1a,*J2l,*J2a;
|
||||
|
||||
// elements to jump from one row to the next in J's
|
||||
int rowskip;
|
||||
|
||||
// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
|
||||
// "constraint force mixing" vector. c is set to zero on entry, cfm is
|
||||
// set to a constant value (typically very small or zero) value on entry.
|
||||
dReal *c,*cfm;
|
||||
|
||||
// lo and hi limits for variables (set to -/+ infinity on entry).
|
||||
dReal *lo,*hi;
|
||||
|
||||
// findex vector for variables. see the LCP solver interface for a
|
||||
// description of what this does. this is set to -1 on entry.
|
||||
// note that the returned indexes are relative to the first index of
|
||||
// the constraint.
|
||||
int *findex;
|
||||
};
|
||||
|
||||
// virtual function table: size of the joint structure, function pointers.
|
||||
// we do it this way instead of using C++ virtual functions because
|
||||
// sometimes we need to allocate joints ourself within a memory pool.
|
||||
|
||||
typedef void init_fn (dxJoint *joint);
|
||||
typedef void getInfo1_fn (dxJoint *joint, Info1 *info);
|
||||
typedef void getInfo2_fn (dxJoint *joint, Info2 *info);
|
||||
struct Vtable {
|
||||
int size;
|
||||
init_fn *init;
|
||||
getInfo1_fn *getInfo1;
|
||||
getInfo2_fn *getInfo2;
|
||||
int typenum; // a dJointTypeXXX type number
|
||||
};
|
||||
|
||||
Vtable *vtable; // virtual function table
|
||||
int flags; // dJOINT_xxx flags
|
||||
dxJointNode node[2]; // connections to bodies. node[1].body can be 0
|
||||
dJointFeedback *feedback; // optional feedback structure
|
||||
};
|
||||
|
||||
|
||||
// joint group. NOTE: any joints in the group that have their world destroyed
|
||||
// will have their world pointer set to 0.
|
||||
|
||||
struct dxJointGroup : public dBase {
|
||||
int num; // number of joints on the stack
|
||||
dObStack stack; // a stack of (possibly differently sized) dxJoint
|
||||
}; // objects.
|
||||
|
||||
|
||||
// common limit and motor information for a single joint axis of movement
|
||||
struct dxJointLimitMotor {
|
||||
dReal vel,fmax; // powered joint: velocity, max force
|
||||
dReal lostop,histop; // joint limits, relative to initial position
|
||||
dReal fudge_factor; // when powering away from joint limits
|
||||
dReal normal_cfm; // cfm to use when not at a stop
|
||||
dReal stop_erp,stop_cfm; // erp and cfm for when at joint limit
|
||||
dReal bounce; // restitution factor
|
||||
// variables used between getInfo1() and getInfo2()
|
||||
int limit; // 0=free, 1=at lo limit, 2=at hi limit
|
||||
dReal limit_err; // if at limit, amount over limit
|
||||
|
||||
void init (dxWorld *);
|
||||
void set (int num, dReal value);
|
||||
dReal get (int num);
|
||||
int testRotationalLimit (dReal angle);
|
||||
int addLimot (dxJoint *joint, dxJoint::Info2 *info, int row,
|
||||
dVector3 ax1, int rotational);
|
||||
};
|
||||
|
||||
|
||||
// ball and socket
|
||||
|
||||
struct dxJointBall : public dxJoint {
|
||||
dVector3 anchor1; // anchor w.r.t first body
|
||||
dVector3 anchor2; // anchor w.r.t second body
|
||||
};
|
||||
extern struct dxJoint::Vtable __dball_vtable;
|
||||
|
||||
|
||||
// hinge
|
||||
|
||||
struct dxJointHinge : public dxJoint {
|
||||
dVector3 anchor1; // anchor w.r.t first body
|
||||
dVector3 anchor2; // anchor w.r.t second body
|
||||
dVector3 axis1; // axis w.r.t first body
|
||||
dVector3 axis2; // axis w.r.t second body
|
||||
dQuaternion qrel; // initial relative rotation body1 -> body2
|
||||
dxJointLimitMotor limot; // limit and motor information
|
||||
};
|
||||
extern struct dxJoint::Vtable __dhinge_vtable;
|
||||
|
||||
|
||||
// universal
|
||||
|
||||
struct dxJointUniversal : public dxJoint {
|
||||
dVector3 anchor1; // anchor w.r.t first body
|
||||
dVector3 anchor2; // anchor w.r.t second body
|
||||
dVector3 axis1; // axis w.r.t first body
|
||||
dVector3 axis2; // axis w.r.t second body
|
||||
};
|
||||
extern struct dxJoint::Vtable __duniversal_vtable;
|
||||
|
||||
|
||||
// slider. if body2 is 0 then qrel is the absolute rotation of body1 and
|
||||
// offset is the position of body1 center along axis1.
|
||||
|
||||
struct dxJointSlider : public dxJoint {
|
||||
dVector3 axis1; // axis w.r.t first body
|
||||
dQuaternion qrel; // initial relative rotation body1 -> body2
|
||||
dVector3 offset; // point relative to body2 that should be
|
||||
// aligned with body1 center along axis1
|
||||
dxJointLimitMotor limot; // limit and motor information
|
||||
};
|
||||
extern struct dxJoint::Vtable __dslider_vtable;
|
||||
|
||||
|
||||
// contact
|
||||
|
||||
struct dxJointContact : public dxJoint {
|
||||
int the_m; // number of rows computed by getInfo1
|
||||
dContact contact;
|
||||
};
|
||||
extern struct dxJoint::Vtable __dcontact_vtable;
|
||||
|
||||
|
||||
// hinge 2
|
||||
|
||||
struct dxJointHinge2 : public dxJoint {
|
||||
dVector3 anchor1; // anchor w.r.t first body
|
||||
dVector3 anchor2; // anchor w.r.t second body
|
||||
dVector3 axis1; // axis 1 w.r.t first body
|
||||
dVector3 axis2; // axis 2 w.r.t second body
|
||||
dReal c0,s0; // cos,sin of desired angle between axis 1,2
|
||||
dVector3 v1,v2; // angle ref vectors embedded in first body
|
||||
dxJointLimitMotor limot1; // limit+motor info for axis 1
|
||||
dxJointLimitMotor limot2; // limit+motor info for axis 2
|
||||
dReal susp_erp,susp_cfm; // suspension parameters (erp,cfm)
|
||||
};
|
||||
extern struct dxJoint::Vtable __dhinge2_vtable;
|
||||
|
||||
|
||||
// angular motor
|
||||
|
||||
struct dxJointAMotor : public dxJoint {
|
||||
int num; // number of axes (0..3)
|
||||
int mode; // a dAMotorXXX constant
|
||||
int rel[3]; // what the axes are relative to (global,b1,b2)
|
||||
dVector3 axis[3]; // three axes
|
||||
dxJointLimitMotor limot[3]; // limit+motor info for axes
|
||||
dReal angle[3]; // user-supplied angles for axes
|
||||
// these vectors are used for calculating euler angles
|
||||
dVector3 reference1; // original axis[2], relative to body 1
|
||||
dVector3 reference2; // original axis[0], relative to body 2
|
||||
};
|
||||
extern struct dxJoint::Vtable __damotor_vtable;
|
||||
|
||||
|
||||
// fixed
|
||||
|
||||
struct dxJointFixed : public dxJoint {
|
||||
dVector3 offset; // relative offset between the bodies
|
||||
};
|
||||
extern struct dxJoint::Vtable __dfixed_vtable;
|
||||
|
||||
|
||||
// null joint, for testing only
|
||||
|
||||
struct dxJointNull : public dxJoint {
|
||||
};
|
||||
extern struct dxJoint::Vtable __dnull_vtable;
|
||||
|
||||
#endif
|
||||
|
||||
1455
extern/ode/dist/ode/src/lcp.cpp
vendored
1455
extern/ode/dist/ode/src/lcp.cpp
vendored
@@ -1,1455 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
/*
|
||||
|
||||
|
||||
THE ALGORITHM
|
||||
-------------
|
||||
|
||||
solve A*x = b+w, with x and w subject to certain LCP conditions.
|
||||
each x(i),w(i) must lie on one of the three line segments in the following
|
||||
diagram. each line segment corresponds to one index set :
|
||||
|
||||
w(i)
|
||||
/|\ | :
|
||||
| | :
|
||||
| |i in N :
|
||||
w>0 | |state[i]=0 :
|
||||
| | :
|
||||
| | : i in C
|
||||
w=0 + +-----------------------+
|
||||
| : |
|
||||
| : |
|
||||
w<0 | : |i in N
|
||||
| : |state[i]=1
|
||||
| : |
|
||||
| : |
|
||||
+-------|-----------|-----------|----------> x(i)
|
||||
lo 0 hi
|
||||
|
||||
the Dantzig algorithm proceeds as follows:
|
||||
for i=1:n
|
||||
* if (x(i),w(i)) is not on the line, push x(i) and w(i) positive or
|
||||
negative towards the line. as this is done, the other (x(j),w(j))
|
||||
for j<i are constrained to be on the line. if any (x,w) reaches the
|
||||
end of a line segment then it is switched between index sets.
|
||||
* i is added to the appropriate index set depending on what line segment
|
||||
it hits.
|
||||
|
||||
we restrict lo(i) <= 0 and hi(i) >= 0. this makes the algorithm a bit
|
||||
simpler, because the starting point for x(i),w(i) is always on the dotted
|
||||
line x=0 and x will only ever increase in one direction, so it can only hit
|
||||
two out of the three line segments.
|
||||
|
||||
|
||||
NOTES
|
||||
-----
|
||||
|
||||
this is an implementation of "lcp_dantzig2_ldlt.m" and "lcp_dantzig_lohi.m".
|
||||
the implementation is split into an LCP problem object (dLCP) and an LCP
|
||||
driver function. most optimization occurs in the dLCP object.
|
||||
|
||||
a naive implementation of the algorithm requires either a lot of data motion
|
||||
or a lot of permutation-array lookup, because we are constantly re-ordering
|
||||
rows and columns. to avoid this and make a more optimized algorithm, a
|
||||
non-trivial data structure is used to represent the matrix A (this is
|
||||
implemented in the fast version of the dLCP object).
|
||||
|
||||
during execution of this algorithm, some indexes in A are clamped (set C),
|
||||
some are non-clamped (set N), and some are "don't care" (where x=0).
|
||||
A,x,b,w (and other problem vectors) are permuted such that the clamped
|
||||
indexes are first, the unclamped indexes are next, and the don't-care
|
||||
indexes are last. this permutation is recorded in the array `p'.
|
||||
initially p = 0..n-1, and as the rows and columns of A,x,b,w are swapped,
|
||||
the corresponding elements of p are swapped.
|
||||
|
||||
because the C and N elements are grouped together in the rows of A, we can do
|
||||
lots of work with a fast dot product function. if A,x,etc were not permuted
|
||||
and we only had a permutation array, then those dot products would be much
|
||||
slower as we would have a permutation array lookup in some inner loops.
|
||||
|
||||
A is accessed through an array of row pointers, so that element (i,j) of the
|
||||
permuted matrix is A[i][j]. this makes row swapping fast. for column swapping
|
||||
we still have to actually move the data.
|
||||
|
||||
during execution of this algorithm we maintain an L*D*L' factorization of
|
||||
the clamped submatrix of A (call it `AC') which is the top left nC*nC
|
||||
submatrix of A. there are two ways we could arrange the rows/columns in AC.
|
||||
|
||||
(1) AC is always permuted such that L*D*L' = AC. this causes a problem
|
||||
when a row/column is removed from C, because then all the rows/columns of A
|
||||
between the deleted index and the end of C need to be rotated downward.
|
||||
this results in a lot of data motion and slows things down.
|
||||
(2) L*D*L' is actually a factorization of a *permutation* of AC (which is
|
||||
itself a permutation of the underlying A). this is what we do - the
|
||||
permutation is recorded in the vector C. call this permutation A[C,C].
|
||||
when a row/column is removed from C, all we have to do is swap two
|
||||
rows/columns and manipulate C.
|
||||
|
||||
*/
|
||||
|
||||
#include <ode/common.h>
|
||||
#include "lcp.h"
|
||||
#include <ode/matrix.h>
|
||||
#include <ode/misc.h>
|
||||
#include "mat.h" // for testing
|
||||
#include <ode/timer.h> // for testing
|
||||
|
||||
//***************************************************************************
|
||||
// code generation parameters
|
||||
|
||||
// LCP debugging (mosty for fast dLCP) - this slows things down a lot
|
||||
//#define DEBUG_LCP
|
||||
|
||||
//#define dLCP_SLOW // use slow dLCP object
|
||||
#define dLCP_FAST // use fast dLCP object
|
||||
|
||||
// option 1 : matrix row pointers (less data copying)
|
||||
#define ROWPTRS
|
||||
#define ATYPE dReal **
|
||||
#define AROW(i) (A[i])
|
||||
|
||||
// option 2 : no matrix row pointers (slightly faster inner loops)
|
||||
//#define NOROWPTRS
|
||||
//#define ATYPE dReal *
|
||||
//#define AROW(i) (A+(i)*nskip)
|
||||
|
||||
// misc defines
|
||||
#define ALLOCA dALLOCA16
|
||||
//#define dDot myDot
|
||||
#define NUB_OPTIMIZATIONS
|
||||
|
||||
//***************************************************************************
|
||||
|
||||
// an alternative inline dot product, for speed comparisons
|
||||
|
||||
static inline dReal myDot (dReal *a, dReal *b, int n)
|
||||
{
|
||||
dReal sum=0;
|
||||
while (n > 0) {
|
||||
sum += (*a) * (*b);
|
||||
a++;
|
||||
b++;
|
||||
n--;
|
||||
}
|
||||
return sum;
|
||||
}
|
||||
|
||||
|
||||
// swap row/column i1 with i2 in the n*n matrix A. the leading dimension of
|
||||
// A is nskip. this only references and swaps the lower triangle.
|
||||
// if `do_fast_row_swaps' is nonzero and row pointers are being used, then
|
||||
// rows will be swapped by exchanging row pointers. otherwise the data will
|
||||
// be copied.
|
||||
|
||||
static void swapRowsAndCols (ATYPE A, int n, int i1, int i2, int nskip,
|
||||
int do_fast_row_swaps)
|
||||
{
|
||||
int i;
|
||||
dIASSERT (A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n &&
|
||||
nskip >= n && i1 < i2);
|
||||
|
||||
# ifdef ROWPTRS
|
||||
for (i=i1+1; i<i2; i++) A[i1][i] = A[i][i1];
|
||||
for (i=i1+1; i<i2; i++) A[i][i1] = A[i2][i];
|
||||
A[i1][i2] = A[i1][i1];
|
||||
A[i1][i1] = A[i2][i1];
|
||||
A[i2][i1] = A[i2][i2];
|
||||
// swap rows, by swapping row pointers
|
||||
if (do_fast_row_swaps) {
|
||||
dReal *tmpp;
|
||||
tmpp = A[i1];
|
||||
A[i1] = A[i2];
|
||||
A[i2] = tmpp;
|
||||
}
|
||||
else {
|
||||
dReal *tmprow = (dReal*) ALLOCA (n * sizeof(dReal));
|
||||
memcpy (tmprow,A[i1],n * sizeof(dReal));
|
||||
memcpy (A[i1],A[i2],n * sizeof(dReal));
|
||||
memcpy (A[i2],tmprow,n * sizeof(dReal));
|
||||
}
|
||||
// swap columns the hard way
|
||||
for (i=i2+1; i<n; i++) {
|
||||
dReal tmp = A[i][i1];
|
||||
A[i][i1] = A[i][i2];
|
||||
A[i][i2] = tmp;
|
||||
}
|
||||
# else
|
||||
dReal tmp,*tmprow = (dReal*) ALLOCA (n * sizeof(dReal));
|
||||
if (i1 > 0) {
|
||||
memcpy (tmprow,A+i1*nskip,i1*sizeof(dReal));
|
||||
memcpy (A+i1*nskip,A+i2*nskip,i1*sizeof(dReal));
|
||||
memcpy (A+i2*nskip,tmprow,i1*sizeof(dReal));
|
||||
}
|
||||
for (i=i1+1; i<i2; i++) {
|
||||
tmp = A[i2*nskip+i];
|
||||
A[i2*nskip+i] = A[i*nskip+i1];
|
||||
A[i*nskip+i1] = tmp;
|
||||
}
|
||||
tmp = A[i1*nskip+i1];
|
||||
A[i1*nskip+i1] = A[i2*nskip+i2];
|
||||
A[i2*nskip+i2] = tmp;
|
||||
for (i=i2+1; i<n; i++) {
|
||||
tmp = A[i*nskip+i1];
|
||||
A[i*nskip+i1] = A[i*nskip+i2];
|
||||
A[i*nskip+i2] = tmp;
|
||||
}
|
||||
# endif
|
||||
}
|
||||
|
||||
|
||||
// swap two indexes in the n*n LCP problem. i1 must be <= i2.
|
||||
|
||||
static void swapProblem (ATYPE A, dReal *x, dReal *b, dReal *w, dReal *lo,
|
||||
dReal *hi, int *p, int *state, int *findex,
|
||||
int n, int i1, int i2, int nskip,
|
||||
int do_fast_row_swaps)
|
||||
{
|
||||
dReal tmp;
|
||||
int tmpi;
|
||||
dIASSERT (n>0 && i1 >=0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n &&
|
||||
i1 <= i2);
|
||||
if (i1==i2) return;
|
||||
swapRowsAndCols (A,n,i1,i2,nskip,do_fast_row_swaps);
|
||||
tmp = x[i1];
|
||||
x[i1] = x[i2];
|
||||
x[i2] = tmp;
|
||||
tmp = b[i1];
|
||||
b[i1] = b[i2];
|
||||
b[i2] = tmp;
|
||||
tmp = w[i1];
|
||||
w[i1] = w[i2];
|
||||
w[i2] = tmp;
|
||||
tmp = lo[i1];
|
||||
lo[i1] = lo[i2];
|
||||
lo[i2] = tmp;
|
||||
tmp = hi[i1];
|
||||
hi[i1] = hi[i2];
|
||||
hi[i2] = tmp;
|
||||
tmpi = p[i1];
|
||||
p[i1] = p[i2];
|
||||
p[i2] = tmpi;
|
||||
tmpi = state[i1];
|
||||
state[i1] = state[i2];
|
||||
state[i2] = tmpi;
|
||||
if (findex) {
|
||||
tmpi = findex[i1];
|
||||
findex[i1] = findex[i2];
|
||||
findex[i2] = tmpi;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// for debugging - check that L,d is the factorization of A[C,C].
|
||||
// A[C,C] has size nC*nC and leading dimension nskip.
|
||||
// L has size nC*nC and leading dimension nskip.
|
||||
// d has size nC.
|
||||
|
||||
#ifdef DEBUG_LCP
|
||||
|
||||
static void checkFactorization (ATYPE A, dReal *_L, dReal *_d,
|
||||
int nC, int *C, int nskip)
|
||||
{
|
||||
int i,j;
|
||||
if (nC==0) return;
|
||||
|
||||
// get A1=A, copy the lower triangle to the upper triangle, get A2=A[C,C]
|
||||
dMatrix A1 (nC,nC);
|
||||
for (i=0; i<nC; i++) {
|
||||
for (j=0; j<=i; j++) A1(i,j) = A1(j,i) = AROW(i)[j];
|
||||
}
|
||||
dMatrix A2 = A1.select (nC,C,nC,C);
|
||||
|
||||
// printf ("A1=\n"); A1.print(); printf ("\n");
|
||||
// printf ("A2=\n"); A2.print(); printf ("\n");
|
||||
|
||||
// compute A3 = L*D*L'
|
||||
dMatrix L (nC,nC,_L,nskip,1);
|
||||
dMatrix D (nC,nC);
|
||||
for (i=0; i<nC; i++) D(i,i) = 1/_d[i];
|
||||
L.clearUpperTriangle();
|
||||
for (i=0; i<nC; i++) L(i,i) = 1;
|
||||
dMatrix A3 = L * D * L.transpose();
|
||||
|
||||
// printf ("L=\n"); L.print(); printf ("\n");
|
||||
// printf ("D=\n"); D.print(); printf ("\n");
|
||||
// printf ("A3=\n"); A2.print(); printf ("\n");
|
||||
|
||||
// compare A2 and A3
|
||||
dReal diff = A2.maxDifference (A3);
|
||||
if (diff > 1e-8)
|
||||
dDebug (0,"L*D*L' check, maximum difference = %.6e\n",diff);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
// for debugging
|
||||
|
||||
#ifdef DEBUG_LCP
|
||||
|
||||
static void checkPermutations (int i, int n, int nC, int nN, int *p, int *C)
|
||||
{
|
||||
int j,k;
|
||||
dIASSERT (nC>=0 && nN>=0 && (nC+nN)==i && i < n);
|
||||
for (k=0; k<i; k++) dIASSERT (p[k] >= 0 && p[k] < i);
|
||||
for (k=i; k<n; k++) dIASSERT (p[k] == k);
|
||||
for (j=0; j<nC; j++) {
|
||||
int C_is_bad = 1;
|
||||
for (k=0; k<nC; k++) if (C[k]==j) C_is_bad = 0;
|
||||
dIASSERT (C_is_bad==0);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
//***************************************************************************
|
||||
// dLCP manipulator object. this represents an n*n LCP problem.
|
||||
//
|
||||
// two index sets C and N are kept. each set holds a subset of
|
||||
// the variable indexes 0..n-1. an index can only be in one set.
|
||||
// initially both sets are empty.
|
||||
//
|
||||
// the index set C is special: solutions to A(C,C)\A(C,i) can be generated.
|
||||
|
||||
#ifdef dLCP_SLOW
|
||||
|
||||
// simple but slow implementation of dLCP, for testing the LCP drivers.
|
||||
|
||||
#include "array.h"
|
||||
|
||||
struct dLCP {
|
||||
int n,nub,nskip;
|
||||
dReal *Adata,*x,*b,*w,*lo,*hi; // LCP problem data
|
||||
ATYPE A; // A rows
|
||||
dArray<int> C,N; // index sets
|
||||
int last_i_for_solve1; // last i value given to solve1
|
||||
|
||||
dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
|
||||
dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
|
||||
dReal *_Dell, dReal *_ell, dReal *_tmp,
|
||||
int *_state, int *_findex, int *_p, int *_C, dReal **Arows);
|
||||
// the constructor is given an initial problem description (A,x,b,w) and
|
||||
// space for other working data (which the caller may allocate on the stack).
|
||||
// some of this data is specific to the fast dLCP implementation.
|
||||
// the matrices A and L have size n*n, vectors have size n*1.
|
||||
// A represents a symmetric matrix but only the lower triangle is valid.
|
||||
// `nub' is the number of unbounded indexes at the start. all the indexes
|
||||
// 0..nub-1 will be put into C.
|
||||
|
||||
~dLCP();
|
||||
|
||||
int getNub() { return nub; }
|
||||
// return the value of `nub'. the constructor may want to change it,
|
||||
// so the caller should find out its new value.
|
||||
|
||||
// transfer functions: transfer index i to the given set (C or N). indexes
|
||||
// less than `nub' can never be given. A,x,b,w,etc may be permuted by these
|
||||
// functions, the caller must be robust to this.
|
||||
|
||||
void transfer_i_to_C (int i);
|
||||
// this assumes C and N span 1:i-1. this also assumes that solve1() has
|
||||
// been recently called for the same i without any other transfer
|
||||
// functions in between (thereby allowing some data reuse for the fast
|
||||
// implementation).
|
||||
void transfer_i_to_N (int i);
|
||||
// this assumes C and N span 1:i-1.
|
||||
void transfer_i_from_N_to_C (int i);
|
||||
void transfer_i_from_C_to_N (int i);
|
||||
|
||||
int numC();
|
||||
int numN();
|
||||
// return the number of indexes in set C/N
|
||||
|
||||
int indexC (int i);
|
||||
int indexN (int i);
|
||||
// return index i in set C/N.
|
||||
|
||||
// accessor and arithmetic functions. Aij translates as A(i,j), etc.
|
||||
// make sure that only the lower triangle of A is ever referenced.
|
||||
|
||||
dReal Aii (int i);
|
||||
dReal AiC_times_qC (int i, dReal *q);
|
||||
dReal AiN_times_qN (int i, dReal *q); // for all Nj
|
||||
void pN_equals_ANC_times_qC (dReal *p, dReal *q); // for all Nj
|
||||
void pN_plusequals_ANi (dReal *p, int i, int sign=1);
|
||||
// for all Nj. sign = +1,-1. assumes i > maximum index in N.
|
||||
void pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q);
|
||||
void pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q); // for all Nj
|
||||
void solve1 (dReal *a, int i, int dir=1, int only_transfer=0);
|
||||
// get a(C) = - dir * A(C,C) \ A(C,i). dir must be +/- 1.
|
||||
// the fast version of this function computes some data that is needed by
|
||||
// transfer_i_to_C(). if only_transfer is nonzero then this function
|
||||
// *only* computes that data, it does not set a(C).
|
||||
|
||||
void unpermute();
|
||||
// call this at the end of the LCP function. if the x/w values have been
|
||||
// permuted then this will unscramble them.
|
||||
};
|
||||
|
||||
|
||||
dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
|
||||
dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
|
||||
dReal *_Dell, dReal *_ell, dReal *_tmp,
|
||||
int *_state, int *_findex, int *_p, int *_C, dReal **Arows)
|
||||
{
|
||||
dUASSERT (_findex==0,"slow dLCP object does not support findex array");
|
||||
|
||||
n = _n;
|
||||
nub = _nub;
|
||||
Adata = _Adata;
|
||||
A = 0;
|
||||
x = _x;
|
||||
b = _b;
|
||||
w = _w;
|
||||
lo = _lo;
|
||||
hi = _hi;
|
||||
nskip = dPAD(n);
|
||||
dSetZero (x,n);
|
||||
last_i_for_solve1 = -1;
|
||||
|
||||
int i,j;
|
||||
C.setSize (n);
|
||||
N.setSize (n);
|
||||
for (int i=0; i<n; i++) {
|
||||
C[i] = 0;
|
||||
N[i] = 0;
|
||||
}
|
||||
|
||||
# ifdef ROWPTRS
|
||||
// make matrix row pointers
|
||||
A = Arows;
|
||||
for (i=0; i<n; i++) A[i] = Adata + i*nskip;
|
||||
# else
|
||||
A = Adata;
|
||||
# endif
|
||||
|
||||
// lets make A symmetric
|
||||
for (i=0; i<n; i++) {
|
||||
for (j=i+1; j<n; j++) AROW(i)[j] = AROW(j)[i];
|
||||
}
|
||||
|
||||
// if nub>0, put all indexes 0..nub-1 into C and solve for x
|
||||
if (nub > 0) {
|
||||
for (i=0; i<nub; i++) memcpy (_L+i*nskip,AROW(i),(i+1)*sizeof(dReal));
|
||||
dFactorLDLT (_L,_d,nub,nskip);
|
||||
memcpy (x,b,nub*sizeof(dReal));
|
||||
dSolveLDLT (_L,_d,x,nub,nskip);
|
||||
dSetZero (_w,nub);
|
||||
for (i=0; i<nub; i++) C[i] = 1;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
dLCP::~dLCP()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
void dLCP::transfer_i_to_C (int i)
|
||||
{
|
||||
if (i < nub) dDebug (0,"bad i");
|
||||
if (C[i]) dDebug (0,"i already in C");
|
||||
if (N[i]) dDebug (0,"i already in N");
|
||||
for (int k=0; k<i; k++) {
|
||||
if (!(C[k] ^ N[k])) dDebug (0,"assumptions for C and N violated");
|
||||
}
|
||||
for (int k=i; k<n; k++)
|
||||
if (C[k] || N[k]) dDebug (0,"assumptions for C and N violated");
|
||||
if (i != last_i_for_solve1) dDebug (0,"assumptions for i violated");
|
||||
last_i_for_solve1 = -1;
|
||||
C[i] = 1;
|
||||
}
|
||||
|
||||
|
||||
void dLCP::transfer_i_to_N (int i)
|
||||
{
|
||||
if (i < nub) dDebug (0,"bad i");
|
||||
if (C[i]) dDebug (0,"i already in C");
|
||||
if (N[i]) dDebug (0,"i already in N");
|
||||
for (int k=0; k<i; k++)
|
||||
if (!C[k] && !N[k]) dDebug (0,"assumptions for C and N violated");
|
||||
for (int k=i; k<n; k++)
|
||||
if (C[k] || N[k]) dDebug (0,"assumptions for C and N violated");
|
||||
last_i_for_solve1 = -1;
|
||||
N[i] = 1;
|
||||
}
|
||||
|
||||
|
||||
void dLCP::transfer_i_from_N_to_C (int i)
|
||||
{
|
||||
if (i < nub) dDebug (0,"bad i");
|
||||
if (C[i]) dDebug (0,"i already in C");
|
||||
if (!N[i]) dDebug (0,"i not in N");
|
||||
last_i_for_solve1 = -1;
|
||||
N[i] = 0;
|
||||
C[i] = 1;
|
||||
}
|
||||
|
||||
|
||||
void dLCP::transfer_i_from_C_to_N (int i)
|
||||
{
|
||||
if (i < nub) dDebug (0,"bad i");
|
||||
if (N[i]) dDebug (0,"i already in N");
|
||||
if (!C[i]) dDebug (0,"i not in C");
|
||||
last_i_for_solve1 = -1;
|
||||
C[i] = 0;
|
||||
N[i] = 1;
|
||||
}
|
||||
|
||||
|
||||
int dLCP::numC()
|
||||
{
|
||||
int i,count=0;
|
||||
for (i=0; i<n; i++) if (C[i]) count++;
|
||||
return count;
|
||||
}
|
||||
|
||||
|
||||
int dLCP::numN()
|
||||
{
|
||||
int i,count=0;
|
||||
for (i=0; i<n; i++) if (N[i]) count++;
|
||||
return count;
|
||||
}
|
||||
|
||||
|
||||
int dLCP::indexC (int i)
|
||||
{
|
||||
int k,count=0;
|
||||
for (k=0; k<n; k++) {
|
||||
if (C[k]) {
|
||||
if (count==i) return k;
|
||||
count++;
|
||||
}
|
||||
}
|
||||
dDebug (0,"bad index C (%d)",i);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int dLCP::indexN (int i)
|
||||
{
|
||||
int k,count=0;
|
||||
for (k=0; k<n; k++) {
|
||||
if (N[k]) {
|
||||
if (count==i) return k;
|
||||
count++;
|
||||
}
|
||||
}
|
||||
dDebug (0,"bad index into N");
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
dReal dLCP::Aii (int i)
|
||||
{
|
||||
return AROW(i)[i];
|
||||
}
|
||||
|
||||
|
||||
dReal dLCP::AiC_times_qC (int i, dReal *q)
|
||||
{
|
||||
dReal sum = 0;
|
||||
for (int k=0; k<n; k++) if (C[k]) sum += AROW(i)[k] * q[k];
|
||||
return sum;
|
||||
}
|
||||
|
||||
|
||||
dReal dLCP::AiN_times_qN (int i, dReal *q)
|
||||
{
|
||||
dReal sum = 0;
|
||||
for (int k=0; k<n; k++) if (N[k]) sum += AROW(i)[k] * q[k];
|
||||
return sum;
|
||||
}
|
||||
|
||||
|
||||
void dLCP::pN_equals_ANC_times_qC (dReal *p, dReal *q)
|
||||
{
|
||||
dReal sum;
|
||||
for (int ii=0; ii<n; ii++) if (N[ii]) {
|
||||
sum = 0;
|
||||
for (int jj=0; jj<n; jj++) if (C[jj]) sum += AROW(ii)[jj] * q[jj];
|
||||
p[ii] = sum;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dLCP::pN_plusequals_ANi (dReal *p, int i, int sign)
|
||||
{
|
||||
int k;
|
||||
for (k=0; k<n; k++) if (N[k] && k >= i) dDebug (0,"N assumption violated");
|
||||
if (sign > 0) {
|
||||
for (k=0; k<n; k++) if (N[k]) p[k] += AROW(i)[k];
|
||||
}
|
||||
else {
|
||||
for (k=0; k<n; k++) if (N[k]) p[k] -= AROW(i)[k];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dLCP::pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q)
|
||||
{
|
||||
for (int k=0; k<n; k++) if (C[k]) p[k] += s*q[k];
|
||||
}
|
||||
|
||||
|
||||
void dLCP::pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q)
|
||||
{
|
||||
for (int k=0; k<n; k++) if (N[k]) p[k] += s*q[k];
|
||||
}
|
||||
|
||||
|
||||
void dLCP::solve1 (dReal *a, int i, int dir, int only_transfer)
|
||||
{
|
||||
dReal *AA = (dReal*) ALLOCA (n*nskip*sizeof(dReal));
|
||||
dReal *dd = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *bb = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
int ii,jj,AAi,AAj;
|
||||
|
||||
last_i_for_solve1 = i;
|
||||
AAi = 0;
|
||||
for (ii=0; ii<n; ii++) if (C[ii]) {
|
||||
AAj = 0;
|
||||
for (jj=0; jj<n; jj++) if (C[jj]) {
|
||||
AA[AAi*nskip+AAj] = AROW(ii)[jj];
|
||||
AAj++;
|
||||
}
|
||||
bb[AAi] = AROW(i)[ii];
|
||||
AAi++;
|
||||
}
|
||||
if (AAi==0) return;
|
||||
|
||||
dFactorLDLT (AA,dd,AAi,nskip);
|
||||
dSolveLDLT (AA,dd,bb,AAi,nskip);
|
||||
|
||||
AAi=0;
|
||||
if (dir > 0) {
|
||||
for (ii=0; ii<n; ii++) if (C[ii]) a[ii] = -bb[AAi++];
|
||||
}
|
||||
else {
|
||||
for (ii=0; ii<n; ii++) if (C[ii]) a[ii] = bb[AAi++];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dLCP::unpermute()
|
||||
{
|
||||
}
|
||||
|
||||
#endif // dLCP_SLOW
|
||||
|
||||
//***************************************************************************
|
||||
// fast implementation of dLCP. see the above definition of dLCP for
|
||||
// interface comments.
|
||||
//
|
||||
// `p' records the permutation of A,x,b,w,etc. p is initially 1:n and is
|
||||
// permuted as the other vectors/matrices are permuted.
|
||||
//
|
||||
// A,x,b,w,lo,hi,state,findex,p,c are permuted such that sets C,N have
|
||||
// contiguous indexes. the don't-care indexes follow N.
|
||||
//
|
||||
// an L*D*L' factorization is maintained of A(C,C), and whenever indexes are
|
||||
// added or removed from the set C the factorization is updated.
|
||||
// thus L*D*L'=A[C,C], i.e. a permuted top left nC*nC submatrix of A.
|
||||
// the leading dimension of the matrix L is always `nskip'.
|
||||
//
|
||||
// at the start there may be other indexes that are unbounded but are not
|
||||
// included in `nub'. dLCP will permute the matrix so that absolutely all
|
||||
// unbounded vectors are at the start. thus there may be some initial
|
||||
// permutation.
|
||||
//
|
||||
// the algorithms here assume certain patterns, particularly with respect to
|
||||
// index transfer.
|
||||
|
||||
#ifdef dLCP_FAST
|
||||
|
||||
struct dLCP {
|
||||
int n,nskip,nub;
|
||||
ATYPE A; // A rows
|
||||
dReal *Adata,*x,*b,*w,*lo,*hi; // permuted LCP problem data
|
||||
dReal *L,*d; // L*D*L' factorization of set C
|
||||
dReal *Dell,*ell,*tmp;
|
||||
int *state,*findex,*p,*C;
|
||||
int nC,nN; // size of each index set
|
||||
|
||||
dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
|
||||
dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
|
||||
dReal *_Dell, dReal *_ell, dReal *_tmp,
|
||||
int *_state, int *_findex, int *_p, int *_C, dReal **Arows);
|
||||
int getNub() { return nub; }
|
||||
void transfer_i_to_C (int i);
|
||||
void transfer_i_to_N (int i)
|
||||
{ nN++; } // because we can assume C and N span 1:i-1
|
||||
void transfer_i_from_N_to_C (int i);
|
||||
void transfer_i_from_C_to_N (int i);
|
||||
int numC() { return nC; }
|
||||
int numN() { return nN; }
|
||||
int indexC (int i) { return i; }
|
||||
int indexN (int i) { return i+nC; }
|
||||
dReal Aii (int i) { return AROW(i)[i]; }
|
||||
dReal AiC_times_qC (int i, dReal *q) { return dDot (AROW(i),q,nC); }
|
||||
dReal AiN_times_qN (int i, dReal *q) { return dDot (AROW(i)+nC,q+nC,nN); }
|
||||
void pN_equals_ANC_times_qC (dReal *p, dReal *q);
|
||||
void pN_plusequals_ANi (dReal *p, int i, int sign=1);
|
||||
void pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q)
|
||||
{ for (int i=0; i<nC; i++) p[i] += s*q[i]; }
|
||||
void pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q)
|
||||
{ for (int i=0; i<nN; i++) p[i+nC] += s*q[i+nC]; }
|
||||
void solve1 (dReal *a, int i, int dir=1, int only_transfer=0);
|
||||
void unpermute();
|
||||
};
|
||||
|
||||
|
||||
dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
|
||||
dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
|
||||
dReal *_Dell, dReal *_ell, dReal *_tmp,
|
||||
int *_state, int *_findex, int *_p, int *_C, dReal **Arows)
|
||||
{
|
||||
n = _n;
|
||||
nub = _nub;
|
||||
Adata = _Adata;
|
||||
A = 0;
|
||||
x = _x;
|
||||
b = _b;
|
||||
w = _w;
|
||||
lo = _lo;
|
||||
hi = _hi;
|
||||
L = _L;
|
||||
d = _d;
|
||||
Dell = _Dell;
|
||||
ell = _ell;
|
||||
tmp = _tmp;
|
||||
state = _state;
|
||||
findex = _findex;
|
||||
p = _p;
|
||||
C = _C;
|
||||
nskip = dPAD(n);
|
||||
dSetZero (x,n);
|
||||
|
||||
int k;
|
||||
|
||||
# ifdef ROWPTRS
|
||||
// make matrix row pointers
|
||||
A = Arows;
|
||||
for (k=0; k<n; k++) A[k] = Adata + k*nskip;
|
||||
# else
|
||||
A = Adata;
|
||||
# endif
|
||||
|
||||
nC = 0;
|
||||
nN = 0;
|
||||
for (k=0; k<n; k++) p[k]=k; // initially unpermuted
|
||||
|
||||
/*
|
||||
// for testing, we can do some random swaps in the area i > nub
|
||||
if (nub < n) {
|
||||
for (k=0; k<100; k++) {
|
||||
int i1,i2;
|
||||
do {
|
||||
i1 = dRandInt(n-nub)+nub;
|
||||
i2 = dRandInt(n-nub)+nub;
|
||||
}
|
||||
while (i1 > i2);
|
||||
//printf ("--> %d %d\n",i1,i2);
|
||||
swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i1,i2,nskip,0);
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
// permute the problem so that *all* the unbounded variables are at the
|
||||
// start, i.e. look for unbounded variables not included in `nub'. we can
|
||||
// potentially push up `nub' this way and get a bigger initial factorization.
|
||||
// note that when we swap rows/cols here we must not just swap row pointers,
|
||||
// as the initial factorization relies on the data being all in one chunk.
|
||||
for (k=nub; k<n; k++) {
|
||||
if (lo[k]==-dInfinity && hi[k]==dInfinity) {
|
||||
swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nub,k,nskip,0);
|
||||
nub++;
|
||||
}
|
||||
}
|
||||
|
||||
// if there are unbounded variables at the start, factorize A up to that
|
||||
// point and solve for x. this puts all indexes 0..nub-1 into C.
|
||||
if (nub > 0) {
|
||||
for (k=0; k<nub; k++) memcpy (L+k*nskip,AROW(k),(k+1)*sizeof(dReal));
|
||||
dFactorLDLT (L,d,nub,nskip);
|
||||
memcpy (x,b,nub*sizeof(dReal));
|
||||
dSolveLDLT (L,d,x,nub,nskip);
|
||||
dSetZero (w,nub);
|
||||
for (k=0; k<nub; k++) C[k] = k;
|
||||
nC = nub;
|
||||
}
|
||||
|
||||
// permute the indexes > nub such that all findex variables are at the end
|
||||
if (findex) {
|
||||
int num_at_end = 0;
|
||||
for (k=n-1; k >= nub; k--) {
|
||||
if (findex[k] >= 0) {
|
||||
swapProblem (A,x,b,w,lo,hi,p,state,findex,n,k,n-1-num_at_end,nskip,1);
|
||||
num_at_end++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// print info about indexes
|
||||
/*
|
||||
for (k=0; k<n; k++) {
|
||||
if (k<nub) printf ("C");
|
||||
else if (lo[k]==-dInfinity && hi[k]==dInfinity) printf ("c");
|
||||
else printf (".");
|
||||
}
|
||||
printf ("\n");
|
||||
*/
|
||||
}
|
||||
|
||||
|
||||
void dLCP::transfer_i_to_C (int i)
|
||||
{
|
||||
int j;
|
||||
if (nC > 0) {
|
||||
// ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C))
|
||||
for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j];
|
||||
d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC));
|
||||
}
|
||||
else {
|
||||
d[0] = dRecip (AROW(i)[i]);
|
||||
}
|
||||
swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1);
|
||||
C[nC] = nC;
|
||||
nC++;
|
||||
|
||||
# ifdef DEBUG_LCP
|
||||
checkFactorization (A,L,d,nC,C,nskip);
|
||||
if (i < (n-1)) checkPermutations (i+1,n,nC,nN,p,C);
|
||||
# endif
|
||||
}
|
||||
|
||||
|
||||
void dLCP::transfer_i_from_N_to_C (int i)
|
||||
{
|
||||
int j;
|
||||
if (nC > 0) {
|
||||
dReal *aptr = AROW(i);
|
||||
# ifdef NUB_OPTIMIZATIONS
|
||||
// if nub>0, initial part of aptr unpermuted
|
||||
for (j=0; j<nub; j++) Dell[j] = aptr[j];
|
||||
for (j=nub; j<nC; j++) Dell[j] = aptr[C[j]];
|
||||
# else
|
||||
for (j=0; j<nC; j++) Dell[j] = aptr[C[j]];
|
||||
# endif
|
||||
dSolveL1 (L,Dell,nC,nskip);
|
||||
for (j=0; j<nC; j++) ell[j] = Dell[j] * d[j];
|
||||
for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j];
|
||||
d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC));
|
||||
}
|
||||
else {
|
||||
d[0] = dRecip (AROW(i)[i]);
|
||||
}
|
||||
swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1);
|
||||
C[nC] = nC;
|
||||
nN--;
|
||||
nC++;
|
||||
|
||||
// @@@ TO DO LATER
|
||||
// if we just finish here then we'll go back and re-solve for
|
||||
// delta_x. but actually we can be more efficient and incrementally
|
||||
// update delta_x here. but if we do this, we wont have ell and Dell
|
||||
// to use in updating the factorization later.
|
||||
|
||||
# ifdef DEBUG_LCP
|
||||
checkFactorization (A,L,d,nC,C,nskip);
|
||||
# endif
|
||||
}
|
||||
|
||||
|
||||
void dLCP::transfer_i_from_C_to_N (int i)
|
||||
{
|
||||
// remove a row/column from the factorization, and adjust the
|
||||
// indexes (black magic!)
|
||||
int j,k;
|
||||
for (j=0; j<nC; j++) if (C[j]==i) {
|
||||
dLDLTRemove (A,C,L,d,n,nC,j,nskip);
|
||||
for (k=0; k<nC; k++) if (C[k]==nC-1) {
|
||||
C[k] = C[j];
|
||||
if (j < (nC-1)) memmove (C+j,C+j+1,(nC-j-1)*sizeof(int));
|
||||
break;
|
||||
}
|
||||
dIASSERT (k < nC);
|
||||
break;
|
||||
}
|
||||
dIASSERT (j < nC);
|
||||
swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i,nC-1,nskip,1);
|
||||
nC--;
|
||||
nN++;
|
||||
|
||||
# ifdef DEBUG_LCP
|
||||
checkFactorization (A,L,d,nC,C,nskip);
|
||||
# endif
|
||||
}
|
||||
|
||||
|
||||
void dLCP::pN_equals_ANC_times_qC (dReal *p, dReal *q)
|
||||
{
|
||||
// we could try to make this matrix-vector multiplication faster using
|
||||
// outer product matrix tricks, e.g. with the dMultidotX() functions.
|
||||
// but i tried it and it actually made things slower on random 100x100
|
||||
// problems because of the overhead involved. so we'll stick with the
|
||||
// simple method for now.
|
||||
for (int i=0; i<nN; i++) p[i+nC] = dDot (AROW(i+nC),q,nC);
|
||||
}
|
||||
|
||||
|
||||
void dLCP::pN_plusequals_ANi (dReal *p, int i, int sign)
|
||||
{
|
||||
dReal *aptr = AROW(i)+nC;
|
||||
if (sign > 0) {
|
||||
for (int i=0; i<nN; i++) p[i+nC] += aptr[i];
|
||||
}
|
||||
else {
|
||||
for (int i=0; i<nN; i++) p[i+nC] -= aptr[i];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dLCP::solve1 (dReal *a, int i, int dir, int only_transfer)
|
||||
{
|
||||
// the `Dell' and `ell' that are computed here are saved. if index i is
|
||||
// later added to the factorization then they can be reused.
|
||||
//
|
||||
// @@@ question: do we need to solve for entire delta_x??? yes, but
|
||||
// only if an x goes below 0 during the step.
|
||||
|
||||
int j;
|
||||
if (nC > 0) {
|
||||
dReal *aptr = AROW(i);
|
||||
# ifdef NUB_OPTIMIZATIONS
|
||||
// if nub>0, initial part of aptr[] is guaranteed unpermuted
|
||||
for (j=0; j<nub; j++) Dell[j] = aptr[j];
|
||||
for (j=nub; j<nC; j++) Dell[j] = aptr[C[j]];
|
||||
# else
|
||||
for (j=0; j<nC; j++) Dell[j] = aptr[C[j]];
|
||||
# endif
|
||||
dSolveL1 (L,Dell,nC,nskip);
|
||||
for (j=0; j<nC; j++) ell[j] = Dell[j] * d[j];
|
||||
|
||||
if (!only_transfer) {
|
||||
for (j=0; j<nC; j++) tmp[j] = ell[j];
|
||||
dSolveL1T (L,tmp,nC,nskip);
|
||||
if (dir > 0) {
|
||||
for (j=0; j<nC; j++) a[C[j]] = -tmp[j];
|
||||
}
|
||||
else {
|
||||
for (j=0; j<nC; j++) a[C[j]] = tmp[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dLCP::unpermute()
|
||||
{
|
||||
// now we have to un-permute x and w
|
||||
int j;
|
||||
dReal *tmp = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
memcpy (tmp,x,n*sizeof(dReal));
|
||||
for (j=0; j<n; j++) x[p[j]] = tmp[j];
|
||||
memcpy (tmp,w,n*sizeof(dReal));
|
||||
for (j=0; j<n; j++) w[p[j]] = tmp[j];
|
||||
}
|
||||
|
||||
#endif // dLCP_FAST
|
||||
|
||||
//***************************************************************************
|
||||
// an unoptimized Dantzig LCP driver routine for the basic LCP problem.
|
||||
// must have lo=0, hi=dInfinity, and nub=0.
|
||||
|
||||
void dSolveLCPBasic (int n, dReal *A, dReal *x, dReal *b,
|
||||
dReal *w, int nub, dReal *lo, dReal *hi)
|
||||
{
|
||||
dAASSERT (n>0 && A && x && b && w && nub == 0);
|
||||
|
||||
int i,k;
|
||||
int nskip = dPAD(n);
|
||||
dReal *L = (dReal*) ALLOCA (n*nskip*sizeof(dReal));
|
||||
dReal *d = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *delta_x = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *delta_w = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *Dell = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *ell = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *tmp = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal **Arows = (dReal**) ALLOCA (n*sizeof(dReal*));
|
||||
int *p = (int*) ALLOCA (n*sizeof(int));
|
||||
int *C = (int*) ALLOCA (n*sizeof(int));
|
||||
int *dummy = (int*) ALLOCA (n*sizeof(int));
|
||||
|
||||
dLCP lcp (n,0,A,x,b,w,tmp,tmp,L,d,Dell,ell,tmp,dummy,dummy,p,C,Arows);
|
||||
nub = lcp.getNub();
|
||||
|
||||
for (i=0; i<n; i++) {
|
||||
w[i] = lcp.AiC_times_qC (i,x) - b[i];
|
||||
if (w[i] >= 0) {
|
||||
lcp.transfer_i_to_N (i);
|
||||
}
|
||||
else {
|
||||
for (;;) {
|
||||
// compute: delta_x(C) = -A(C,C)\A(C,i)
|
||||
dSetZero (delta_x,n);
|
||||
lcp.solve1 (delta_x,i);
|
||||
delta_x[i] = 1;
|
||||
|
||||
// compute: delta_w = A*delta_x
|
||||
dSetZero (delta_w,n);
|
||||
lcp.pN_equals_ANC_times_qC (delta_w,delta_x);
|
||||
lcp.pN_plusequals_ANi (delta_w,i);
|
||||
delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i);
|
||||
|
||||
// find index to switch
|
||||
int si = i; // si = switch index
|
||||
int si_in_N = 0; // set to 1 if si in N
|
||||
dReal s = -w[i]/delta_w[i];
|
||||
|
||||
if (s <= 0) {
|
||||
dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s);
|
||||
if (i < (n-1)) {
|
||||
dSetZero (x+i,n-i);
|
||||
dSetZero (w+i,n-i);
|
||||
}
|
||||
goto done;
|
||||
}
|
||||
|
||||
for (k=0; k < lcp.numN(); k++) {
|
||||
if (delta_w[lcp.indexN(k)] < 0) {
|
||||
dReal s2 = -w[lcp.indexN(k)] / delta_w[lcp.indexN(k)];
|
||||
if (s2 < s) {
|
||||
s = s2;
|
||||
si = lcp.indexN(k);
|
||||
si_in_N = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
for (k=0; k < lcp.numC(); k++) {
|
||||
if (delta_x[lcp.indexC(k)] < 0) {
|
||||
dReal s2 = -x[lcp.indexC(k)] / delta_x[lcp.indexC(k)];
|
||||
if (s2 < s) {
|
||||
s = s2;
|
||||
si = lcp.indexC(k);
|
||||
si_in_N = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// apply x = x + s * delta_x
|
||||
lcp.pC_plusequals_s_times_qC (x,s,delta_x);
|
||||
x[i] += s;
|
||||
lcp.pN_plusequals_s_times_qN (w,s,delta_w);
|
||||
w[i] += s * delta_w[i];
|
||||
|
||||
// switch indexes between sets if necessary
|
||||
if (si==i) {
|
||||
w[i] = 0;
|
||||
lcp.transfer_i_to_C (i);
|
||||
break;
|
||||
}
|
||||
if (si_in_N) {
|
||||
w[si] = 0;
|
||||
lcp.transfer_i_from_N_to_C (si);
|
||||
}
|
||||
else {
|
||||
x[si] = 0;
|
||||
lcp.transfer_i_from_C_to_N (si);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
done:
|
||||
lcp.unpermute();
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
// an optimized Dantzig LCP driver routine for the lo-hi LCP problem.
|
||||
|
||||
void dSolveLCP (int n, dReal *A, dReal *x, dReal *b,
|
||||
dReal *w, int nub, dReal *lo, dReal *hi, int *findex)
|
||||
{
|
||||
dAASSERT (n>0 && A && x && b && w && lo && hi && nub >= 0 && nub <= n);
|
||||
int i,k,hit_first_friction_index = 0;
|
||||
int nskip = dPAD(n);
|
||||
|
||||
// if all the variables are unbounded then we can just factor, solve,
|
||||
// and return
|
||||
if (nub >= n) {
|
||||
dFactorLDLT (A,w,n,nskip); // use w for d
|
||||
dSolveLDLT (A,w,b,n,nskip);
|
||||
memcpy (x,b,n*sizeof(dReal));
|
||||
dSetZero (w,n);
|
||||
return;
|
||||
}
|
||||
|
||||
# ifndef dNODEBUG
|
||||
// check restrictions on lo and hi
|
||||
for (k=0; k<n; k++) dIASSERT (lo[k] <= 0 && hi[k] >= 0);
|
||||
# endif
|
||||
|
||||
dReal *L = (dReal*) ALLOCA (n*nskip*sizeof(dReal));
|
||||
dReal *d = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *delta_x = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *delta_w = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *Dell = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *ell = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal **Arows = (dReal**) ALLOCA (n*sizeof(dReal*));
|
||||
int *p = (int*) ALLOCA (n*sizeof(int));
|
||||
int *C = (int*) ALLOCA (n*sizeof(int));
|
||||
int dir;
|
||||
dReal dirf;
|
||||
|
||||
// for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
|
||||
int *state = (int*) ALLOCA (n*sizeof(int));
|
||||
|
||||
// create LCP object. note that tmp is set to delta_w to save space, this
|
||||
// optimization relies on knowledge of how tmp is used, so be careful!
|
||||
dLCP lcp (n,nub,A,x,b,w,lo,hi,L,d,Dell,ell,delta_w,state,findex,p,C,Arows);
|
||||
nub = lcp.getNub();
|
||||
|
||||
// loop over all indexes nub..n-1. for index i, if x(i),w(i) satisfy the
|
||||
// LCP conditions then i is added to the appropriate index set. otherwise
|
||||
// x(i),w(i) is driven either +ve or -ve to force it to the valid region.
|
||||
// as we drive x(i), x(C) is also adjusted to keep w(C) at zero.
|
||||
// while driving x(i) we maintain the LCP conditions on the other variables
|
||||
// 0..i-1. we do this by watching out for other x(i),w(i) values going
|
||||
// outside the valid region, and then switching them between index sets
|
||||
// when that happens.
|
||||
|
||||
for (i=nub; i<n; i++) {
|
||||
// the index i is the driving index and indexes i+1..n-1 are "dont care",
|
||||
// i.e. when we make changes to the system those x's will be zero and we
|
||||
// don't care what happens to those w's. in other words, we only consider
|
||||
// an (i+1)*(i+1) sub-problem of A*x=b+w.
|
||||
|
||||
// if we've hit the first friction index, we have to compute the lo and
|
||||
// hi values based on the values of x already computed. we have been
|
||||
// permuting the indexes, so the values stored in the findex vector are
|
||||
// no longer valid. thus we have to temporarily unpermute the x vector.
|
||||
if (hit_first_friction_index == 0 && findex && findex[i] >= 0) {
|
||||
// un-permute x into delta_w, which is not being used at the moment
|
||||
for (k=0; k<n; k++) delta_w[p[k]] = x[k];
|
||||
// set lo and hi values
|
||||
for (k=i; k<n; k++) {
|
||||
hi[k] = dFabs (hi[k] * delta_w[findex[k]]);
|
||||
lo[k] = -hi[k];
|
||||
}
|
||||
hit_first_friction_index = 1;
|
||||
}
|
||||
|
||||
// thus far we have not even been computing the w values for indexes
|
||||
// greater than i, so compute w[i] now.
|
||||
w[i] = lcp.AiC_times_qC (i,x) + lcp.AiN_times_qN (i,x) - b[i];
|
||||
|
||||
// if lo=hi=0 (which can happen for tangential friction when normals are
|
||||
// 0) then the index will be assigned to set N with some state. however,
|
||||
// set C's line has zero size, so the index will always remain in set N.
|
||||
// with the "normal" switching logic, if w changed sign then the index
|
||||
// would have to switch to set C and then back to set N with an inverted
|
||||
// state. this is pointless, and also computationally expensive. to
|
||||
// prevent this from happening, we use the rule that indexes with lo=hi=0
|
||||
// will never be checked for set changes. this means that the state for
|
||||
// these indexes may be incorrect, but that doesn't matter.
|
||||
|
||||
// see if x(i),w(i) is in a valid region
|
||||
if (lo[i]==0 && w[i] >= 0) {
|
||||
lcp.transfer_i_to_N (i);
|
||||
state[i] = 0;
|
||||
}
|
||||
else if (hi[i]==0 && w[i] <= 0) {
|
||||
lcp.transfer_i_to_N (i);
|
||||
state[i] = 1;
|
||||
}
|
||||
else if (w[i]==0) {
|
||||
// this is a degenerate case. by the time we get to this test we know
|
||||
// that lo != 0, which means that lo < 0 as lo is not allowed to be +ve,
|
||||
// and similarly that hi > 0. this means that the line segment
|
||||
// corresponding to set C is at least finite in extent, and we are on it.
|
||||
// NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C()
|
||||
lcp.solve1 (delta_x,i,0,1);
|
||||
lcp.transfer_i_to_C (i);
|
||||
}
|
||||
else {
|
||||
// we must push x(i) and w(i)
|
||||
for (;;) {
|
||||
// find direction to push on x(i)
|
||||
if (w[i] <= 0) {
|
||||
dir = 1;
|
||||
dirf = REAL(1.0);
|
||||
}
|
||||
else {
|
||||
dir = -1;
|
||||
dirf = REAL(-1.0);
|
||||
}
|
||||
|
||||
// compute: delta_x(C) = -dir*A(C,C)\A(C,i)
|
||||
lcp.solve1 (delta_x,i,dir);
|
||||
// note that delta_x[i] = dirf, but we wont bother to set it
|
||||
|
||||
// compute: delta_w = A*delta_x ... note we only care about
|
||||
// delta_w(N) and delta_w(i), the rest is ignored
|
||||
lcp.pN_equals_ANC_times_qC (delta_w,delta_x);
|
||||
lcp.pN_plusequals_ANi (delta_w,i,dir);
|
||||
delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i)*dirf;
|
||||
|
||||
// find largest step we can take (size=s), either to drive x(i),w(i)
|
||||
// to the valid LCP region or to drive an already-valid variable
|
||||
// outside the valid region.
|
||||
|
||||
int cmd = 1; // index switching command
|
||||
int si = 0; // si = index to switch if cmd>3
|
||||
dReal s = -w[i]/delta_w[i];
|
||||
if (dir > 0) {
|
||||
if (hi[i] < dInfinity) {
|
||||
dReal s2 = (hi[i]-x[i])/dirf; // step to x(i)=hi(i)
|
||||
if (s2 < s) {
|
||||
s = s2;
|
||||
cmd = 3;
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
if (lo[i] > -dInfinity) {
|
||||
dReal s2 = (lo[i]-x[i])/dirf; // step to x(i)=lo(i)
|
||||
if (s2 < s) {
|
||||
s = s2;
|
||||
cmd = 2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (k=0; k < lcp.numN(); k++) {
|
||||
if ((state[lcp.indexN(k)]==0 && delta_w[lcp.indexN(k)] < 0) ||
|
||||
(state[lcp.indexN(k)]!=0 && delta_w[lcp.indexN(k)] > 0)) {
|
||||
// don't bother checking if lo=hi=0
|
||||
if (lo[lcp.indexN(k)] == 0 && hi[lcp.indexN(k)] == 0) continue;
|
||||
dReal s2 = -w[lcp.indexN(k)] / delta_w[lcp.indexN(k)];
|
||||
if (s2 < s) {
|
||||
s = s2;
|
||||
cmd = 4;
|
||||
si = lcp.indexN(k);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (k=nub; k < lcp.numC(); k++) {
|
||||
if (delta_x[lcp.indexC(k)] < 0 && lo[lcp.indexC(k)] > -dInfinity) {
|
||||
dReal s2 = (lo[lcp.indexC(k)]-x[lcp.indexC(k)]) /
|
||||
delta_x[lcp.indexC(k)];
|
||||
if (s2 < s) {
|
||||
s = s2;
|
||||
cmd = 5;
|
||||
si = lcp.indexC(k);
|
||||
}
|
||||
}
|
||||
if (delta_x[lcp.indexC(k)] > 0 && hi[lcp.indexC(k)] < dInfinity) {
|
||||
dReal s2 = (hi[lcp.indexC(k)]-x[lcp.indexC(k)]) /
|
||||
delta_x[lcp.indexC(k)];
|
||||
if (s2 < s) {
|
||||
s = s2;
|
||||
cmd = 6;
|
||||
si = lcp.indexC(k);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C",
|
||||
// "C->NL","C->NH"};
|
||||
//printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i);
|
||||
|
||||
// if s <= 0 then we've got a problem. if we just keep going then
|
||||
// we're going to get stuck in an infinite loop. instead, just cross
|
||||
// our fingers and exit with the current solution.
|
||||
if (s <= 0) {
|
||||
dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s);
|
||||
if (i < (n-1)) {
|
||||
dSetZero (x+i,n-i);
|
||||
dSetZero (w+i,n-i);
|
||||
}
|
||||
goto done;
|
||||
}
|
||||
|
||||
// apply x = x + s * delta_x
|
||||
lcp.pC_plusequals_s_times_qC (x,s,delta_x);
|
||||
x[i] += s * dirf;
|
||||
|
||||
// apply w = w + s * delta_w
|
||||
lcp.pN_plusequals_s_times_qN (w,s,delta_w);
|
||||
w[i] += s * delta_w[i];
|
||||
|
||||
// switch indexes between sets if necessary
|
||||
switch (cmd) {
|
||||
case 1: // done
|
||||
w[i] = 0;
|
||||
lcp.transfer_i_to_C (i);
|
||||
break;
|
||||
case 2: // done
|
||||
x[i] = lo[i];
|
||||
state[i] = 0;
|
||||
lcp.transfer_i_to_N (i);
|
||||
break;
|
||||
case 3: // done
|
||||
x[i] = hi[i];
|
||||
state[i] = 1;
|
||||
lcp.transfer_i_to_N (i);
|
||||
break;
|
||||
case 4: // keep going
|
||||
w[si] = 0;
|
||||
lcp.transfer_i_from_N_to_C (si);
|
||||
break;
|
||||
case 5: // keep going
|
||||
x[si] = lo[si];
|
||||
state[si] = 0;
|
||||
lcp.transfer_i_from_C_to_N (si);
|
||||
break;
|
||||
case 6: // keep going
|
||||
x[si] = hi[si];
|
||||
state[si] = 1;
|
||||
lcp.transfer_i_from_C_to_N (si);
|
||||
break;
|
||||
}
|
||||
|
||||
if (cmd <= 3) break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
done:
|
||||
lcp.unpermute();
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
// accuracy and timing test
|
||||
|
||||
extern "C" void dTestSolveLCP()
|
||||
{
|
||||
int n = 100;
|
||||
int i,nskip = dPAD(n);
|
||||
const dReal tol = REAL(1e-9);
|
||||
printf ("dTestSolveLCP()\n");
|
||||
|
||||
dReal *A = (dReal*) ALLOCA (n*nskip*sizeof(dReal));
|
||||
dReal *x = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *b = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *w = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *lo = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *hi = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
|
||||
dReal *A2 = (dReal*) ALLOCA (n*nskip*sizeof(dReal));
|
||||
dReal *b2 = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *lo2 = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *hi2 = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *tmp1 = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
dReal *tmp2 = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
|
||||
double total_time = 0;
|
||||
for (int count=0; count < 1000; count++) {
|
||||
|
||||
// form (A,b) = a random positive definite LCP problem
|
||||
dMakeRandomMatrix (A2,n,n,1.0);
|
||||
dMultiply2 (A,A2,A2,n,n,n);
|
||||
dMakeRandomMatrix (x,n,1,1.0);
|
||||
dMultiply0 (b,A,x,n,n,1);
|
||||
for (i=0; i<n; i++) b[i] += (dRandReal()*REAL(0.2))-REAL(0.1);
|
||||
|
||||
// choose `nub' in the range 0..n-1
|
||||
int nub = 50; //dRandInt (n);
|
||||
|
||||
// make limits
|
||||
for (i=0; i<nub; i++) lo[i] = -dInfinity;
|
||||
for (i=0; i<nub; i++) hi[i] = dInfinity;
|
||||
//for (i=nub; i<n; i++) lo[i] = 0;
|
||||
//for (i=nub; i<n; i++) hi[i] = dInfinity;
|
||||
//for (i=nub; i<n; i++) lo[i] = -dInfinity;
|
||||
//for (i=nub; i<n; i++) hi[i] = 0;
|
||||
for (i=nub; i<n; i++) lo[i] = -(dRandReal()*REAL(1.0))-REAL(0.01);
|
||||
for (i=nub; i<n; i++) hi[i] = (dRandReal()*REAL(1.0))+REAL(0.01);
|
||||
|
||||
// set a few limits to lo=hi=0
|
||||
/*
|
||||
for (i=0; i<10; i++) {
|
||||
int j = dRandInt (n-nub) + nub;
|
||||
lo[j] = 0;
|
||||
hi[j] = 0;
|
||||
}
|
||||
*/
|
||||
|
||||
// solve the LCP. we must make copy of A,b,lo,hi (A2,b2,lo2,hi2) for
|
||||
// SolveLCP() to permute. also, we'll clear the upper triangle of A2 to
|
||||
// ensure that it doesn't get referenced (if it does, the answer will be
|
||||
// wrong).
|
||||
|
||||
memcpy (A2,A,n*nskip*sizeof(dReal));
|
||||
dClearUpperTriangle (A2,n);
|
||||
memcpy (b2,b,n*sizeof(dReal));
|
||||
memcpy (lo2,lo,n*sizeof(dReal));
|
||||
memcpy (hi2,hi,n*sizeof(dReal));
|
||||
dSetZero (x,n);
|
||||
dSetZero (w,n);
|
||||
|
||||
dStopwatch sw;
|
||||
dStopwatchReset (&sw);
|
||||
dStopwatchStart (&sw);
|
||||
|
||||
dSolveLCP (n,A2,x,b2,w,nub,lo2,hi2,0);
|
||||
|
||||
dStopwatchStop (&sw);
|
||||
double time = dStopwatchTime(&sw);
|
||||
total_time += time;
|
||||
double average = total_time / double(count+1) * 1000.0;
|
||||
|
||||
// check the solution
|
||||
|
||||
dMultiply0 (tmp1,A,x,n,n,1);
|
||||
for (i=0; i<n; i++) tmp2[i] = b[i] + w[i];
|
||||
dReal diff = dMaxDifference (tmp1,tmp2,n,1);
|
||||
// printf ("\tA*x = b+w, maximum difference = %.6e - %s (1)\n",diff,
|
||||
// diff > tol ? "FAILED" : "passed");
|
||||
if (diff > tol) dDebug (0,"A*x = b+w, maximum difference = %.6e",diff);
|
||||
int n1=0,n2=0,n3=0;
|
||||
for (i=0; i<n; i++) {
|
||||
if (x[i]==lo[i] && w[i] >= 0) {
|
||||
n1++; // ok
|
||||
}
|
||||
else if (x[i]==hi[i] && w[i] <= 0) {
|
||||
n2++; // ok
|
||||
}
|
||||
else if (x[i] >= lo[i] && x[i] <= hi[i] && w[i] == 0) {
|
||||
n3++; // ok
|
||||
}
|
||||
else {
|
||||
dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i,
|
||||
x[i],w[i],lo[i],hi[i]);
|
||||
}
|
||||
}
|
||||
|
||||
// pacifier
|
||||
printf ("passed: NL=%3d NH=%3d C=%3d ",n1,n2,n3);
|
||||
printf ("time=%10.3f ms avg=%10.4f\n",time * 1000.0,average);
|
||||
}
|
||||
}
|
||||
59
extern/ode/dist/ode/src/lcp.h
vendored
59
extern/ode/dist/ode/src/lcp.h
vendored
@@ -1,59 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
/*
|
||||
|
||||
given (A,b,lo,hi), solve the LCP problem: A*x = b+w, where each x(i),w(i)
|
||||
satisfies one of
|
||||
(1) x = lo, w >= 0
|
||||
(2) x = hi, w <= 0
|
||||
(3) lo < x < hi, w = 0
|
||||
A is a matrix of dimension n*n, everything else is a vector of size n*1.
|
||||
lo and hi can be +/- dInfinity as needed. the first `nub' variables are
|
||||
unbounded, i.e. hi and lo are assumed to be +/- dInfinity.
|
||||
|
||||
we restrict lo(i) <= 0 and hi(i) >= 0.
|
||||
|
||||
the original data (A,b) may be modified by this function.
|
||||
|
||||
if the `findex' (friction index) parameter is nonzero, it points to an array
|
||||
of index values. in this case constraints that have findex[i] >= 0 are
|
||||
special. all non-special constraints are solved for, then the lo and hi values
|
||||
for the special constraints are set:
|
||||
hi[i] = abs( hi[i] * x[findex[i]] )
|
||||
lo[i] = -hi[i]
|
||||
and the solution continues. this mechanism allows a friction approximation
|
||||
to be implemented.
|
||||
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _ODE_LCP_H_
|
||||
#define _ODE_LCP_H_
|
||||
|
||||
|
||||
void dSolveLCP (int n, dReal *A, dReal *x, dReal *b, dReal *w,
|
||||
int nub, dReal *lo, dReal *hi, int *findex);
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
261
extern/ode/dist/ode/src/mass.cpp
vendored
261
extern/ode/dist/ode/src/mass.cpp
vendored
@@ -1,261 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#include <ode/config.h>
|
||||
#include <ode/mass.h>
|
||||
#include <ode/odemath.h>
|
||||
#include <ode/matrix.h>
|
||||
|
||||
|
||||
#define _I(i,j) I[(i)*4+(j)]
|
||||
|
||||
|
||||
// return 1 if ok, 0 if bad
|
||||
|
||||
static int checkMass (dMass *m)
|
||||
{
|
||||
if (m->mass <= 0) {
|
||||
dDEBUGMSG ("mass must be > 0");
|
||||
return 0;
|
||||
}
|
||||
if (!dIsPositiveDefinite (m->I,3)) {
|
||||
dDEBUGMSG ("inertia must be positive definite");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// verify that the center of mass position is consistent with the mass
|
||||
// and inertia matrix. this is done by checking that the inertia around
|
||||
// the center of mass is also positive definite. from the comment in
|
||||
// dMassTranslate(), if the body is translated so that its center of mass
|
||||
// is at the point of reference, then the new inertia is:
|
||||
// I + mass*crossmat(c)^2
|
||||
// note that requiring this to be positive definite is exactly equivalent
|
||||
// to requiring that the spatial inertia matrix
|
||||
// [ mass*eye(3,3) M*crossmat(c)^T ]
|
||||
// [ M*crossmat(c) I ]
|
||||
// is positive definite, given that I is PD and mass>0. see the theorem
|
||||
// about partitioned PD matrices for proof.
|
||||
|
||||
dMatrix3 I2,chat;
|
||||
dSetZero (chat,12);
|
||||
dCROSSMAT (chat,m->c,4,+,-);
|
||||
dMULTIPLY0_333 (I2,chat,chat);
|
||||
for (int i=0; i<12; i++) I2[i] = m->I[i] + m->mass*I2[i];
|
||||
if (!dIsPositiveDefinite (I2,3)) {
|
||||
dDEBUGMSG ("center of mass inconsistent with mass parameters");
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
void dMassSetZero (dMass *m)
|
||||
{
|
||||
dAASSERT (m);
|
||||
m->mass = REAL(0.0);
|
||||
dSetZero (m->c,sizeof(m->c) / sizeof(dReal));
|
||||
dSetZero (m->I,sizeof(m->I) / sizeof(dReal));
|
||||
}
|
||||
|
||||
|
||||
void dMassSetParameters (dMass *m, dReal themass,
|
||||
dReal cgx, dReal cgy, dReal cgz,
|
||||
dReal I11, dReal I22, dReal I33,
|
||||
dReal I12, dReal I13, dReal I23)
|
||||
{
|
||||
dAASSERT (m);
|
||||
dMassSetZero (m);
|
||||
m->mass = themass;
|
||||
m->c[0] = cgx;
|
||||
m->c[1] = cgy;
|
||||
m->c[2] = cgz;
|
||||
m->_I(0,0) = I11;
|
||||
m->_I(1,1) = I22;
|
||||
m->_I(2,2) = I33;
|
||||
m->_I(0,1) = I12;
|
||||
m->_I(0,2) = I13;
|
||||
m->_I(1,2) = I23;
|
||||
m->_I(1,0) = I12;
|
||||
m->_I(2,0) = I13;
|
||||
m->_I(2,1) = I23;
|
||||
checkMass (m);
|
||||
}
|
||||
|
||||
|
||||
void dMassSetSphere (dMass *m, dReal density, dReal radius)
|
||||
{
|
||||
dAASSERT (m);
|
||||
dMassSetZero (m);
|
||||
m->mass = (REAL(4.0)/REAL(3.0)) * M_PI * radius*radius*radius * density;
|
||||
dReal II = REAL(0.4) * m->mass * radius*radius;
|
||||
m->_I(0,0) = II;
|
||||
m->_I(1,1) = II;
|
||||
m->_I(2,2) = II;
|
||||
|
||||
# ifndef dNODEBUG
|
||||
checkMass (m);
|
||||
# endif
|
||||
}
|
||||
|
||||
|
||||
void dMassSetCappedCylinder (dMass *m, dReal density, int direction,
|
||||
dReal a, dReal b)
|
||||
{
|
||||
dReal M1,M2,Ia,Ib;
|
||||
dAASSERT (m);
|
||||
dUASSERT (direction >= 1 && direction <= 3,"bad direction number");
|
||||
dMassSetZero (m);
|
||||
M1 = M_PI*a*a*b*density; // cylinder mass
|
||||
M2 = (REAL(4.0)/REAL(3.0))*M_PI*a*a*a*density; // total cap mass
|
||||
m->mass = M1+M2;
|
||||
Ia = M1*(REAL(0.25)*a*a + (REAL(1.0)/REAL(12.0))*b*b) +
|
||||
M2*(REAL(0.4)*a*a + REAL(0.5)*b*b);
|
||||
Ib = (M1*REAL(0.5) + M2*REAL(0.4))*a*a;
|
||||
m->_I(0,0) = Ia;
|
||||
m->_I(1,1) = Ia;
|
||||
m->_I(2,2) = Ia;
|
||||
m->_I(direction-1,direction-1) = Ib;
|
||||
|
||||
# ifndef dNODEBUG
|
||||
checkMass (m);
|
||||
# endif
|
||||
}
|
||||
|
||||
|
||||
void dMassSetBox (dMass *m, dReal density,
|
||||
dReal lx, dReal ly, dReal lz)
|
||||
{
|
||||
dAASSERT (m);
|
||||
dMassSetZero (m);
|
||||
dReal M = lx*ly*lz*density;
|
||||
m->mass = M;
|
||||
m->_I(0,0) = M/REAL(12.0) * (ly*ly + lz*lz);
|
||||
m->_I(1,1) = M/REAL(12.0) * (lx*lx + lz*lz);
|
||||
m->_I(2,2) = M/REAL(12.0) * (lx*lx + ly*ly);
|
||||
|
||||
# ifndef dNODEBUG
|
||||
checkMass (m);
|
||||
# endif
|
||||
}
|
||||
|
||||
|
||||
void dMassAdjust (dMass *m, dReal newmass)
|
||||
{
|
||||
dAASSERT (m);
|
||||
dReal scale = newmass / m->mass;
|
||||
m->mass = newmass;
|
||||
for (int i=0; i<3; i++) for (int j=0; j<3; j++) m->_I(i,j) *= scale;
|
||||
|
||||
# ifndef dNODEBUG
|
||||
checkMass (m);
|
||||
# endif
|
||||
}
|
||||
|
||||
|
||||
void dMassTranslate (dMass *m, dReal x, dReal y, dReal z)
|
||||
{
|
||||
// if the body is translated by `a' relative to its point of reference,
|
||||
// the new inertia about the point of reference is:
|
||||
//
|
||||
// I + mass*(crossmat(c)^2 - crossmat(c+a)^2)
|
||||
//
|
||||
// where c is the existing center of mass and I is the old inertia.
|
||||
|
||||
int i,j;
|
||||
dMatrix3 ahat,chat,t1,t2;
|
||||
dReal a[3];
|
||||
|
||||
dAASSERT (m);
|
||||
|
||||
// adjust inertia matrix
|
||||
dSetZero (chat,12);
|
||||
dCROSSMAT (chat,m->c,4,+,-);
|
||||
a[0] = x + m->c[0];
|
||||
a[1] = y + m->c[1];
|
||||
a[2] = z + m->c[2];
|
||||
dSetZero (ahat,12);
|
||||
dCROSSMAT (ahat,a,4,+,-);
|
||||
dMULTIPLY0_333 (t1,ahat,ahat);
|
||||
dMULTIPLY0_333 (t2,chat,chat);
|
||||
for (i=0; i<3; i++) for (j=0; j<3; j++)
|
||||
m->_I(i,j) += m->mass * (t2[i*4+j]-t1[i*4+j]);
|
||||
|
||||
// ensure perfect symmetry
|
||||
m->_I(1,0) = m->_I(0,1);
|
||||
m->_I(2,0) = m->_I(0,2);
|
||||
m->_I(2,1) = m->_I(1,2);
|
||||
|
||||
// adjust center of mass
|
||||
m->c[0] += x;
|
||||
m->c[1] += y;
|
||||
m->c[2] += z;
|
||||
|
||||
# ifndef dNODEBUG
|
||||
checkMass (m);
|
||||
# endif
|
||||
}
|
||||
|
||||
|
||||
void dMassRotate (dMass *m, const dMatrix3 R)
|
||||
{
|
||||
// if the body is rotated by `R' relative to its point of reference,
|
||||
// the new inertia about the point of reference is:
|
||||
//
|
||||
// R * I * R'
|
||||
//
|
||||
// where I is the old inertia.
|
||||
|
||||
dMatrix3 t1;
|
||||
dReal t2[3];
|
||||
|
||||
dAASSERT (m);
|
||||
|
||||
// rotate inertia matrix
|
||||
dMULTIPLY2_333 (t1,m->I,R);
|
||||
dMULTIPLY0_333 (m->I,R,t1);
|
||||
|
||||
// ensure perfect symmetry
|
||||
m->_I(1,0) = m->_I(0,1);
|
||||
m->_I(2,0) = m->_I(0,2);
|
||||
m->_I(2,1) = m->_I(1,2);
|
||||
|
||||
// rotate center of mass
|
||||
dMULTIPLY0_331 (t2,R,m->c);
|
||||
m->c[0] = t2[0];
|
||||
m->c[1] = t2[1];
|
||||
m->c[2] = t2[2];
|
||||
|
||||
# ifndef dNODEBUG
|
||||
checkMass (m);
|
||||
# endif
|
||||
}
|
||||
|
||||
|
||||
void dMassAdd (dMass *a, const dMass *b)
|
||||
{
|
||||
int i;
|
||||
dAASSERT (a && b);
|
||||
dReal denom = dRecip (a->mass + b->mass);
|
||||
for (i=0; i<3; i++) a->c[i] = (a->c[i]*a->mass + b->c[i]*b->mass)*denom;
|
||||
a->mass += b->mass;
|
||||
for (i=0; i<12; i++) a->I[i] += b->I[i];
|
||||
}
|
||||
230
extern/ode/dist/ode/src/mat.cpp
vendored
230
extern/ode/dist/ode/src/mat.cpp
vendored
@@ -1,230 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#include <ode/config.h>
|
||||
#include <ode/misc.h>
|
||||
#include <ode/matrix.h>
|
||||
#include <ode/error.h>
|
||||
#include <ode/memory.h>
|
||||
#include "mat.h"
|
||||
|
||||
|
||||
dMatrix::dMatrix()
|
||||
{
|
||||
n = 0;
|
||||
m = 0;
|
||||
data = 0;
|
||||
}
|
||||
|
||||
|
||||
dMatrix::dMatrix (int rows, int cols)
|
||||
{
|
||||
if (rows < 1 || cols < 1) dDebug (0,"bad matrix size");
|
||||
n = rows;
|
||||
m = cols;
|
||||
data = (dReal*) dAlloc (n*m*sizeof(dReal));
|
||||
dSetZero (data,n*m);
|
||||
}
|
||||
|
||||
|
||||
dMatrix::dMatrix (const dMatrix &a)
|
||||
{
|
||||
n = a.n;
|
||||
m = a.m;
|
||||
data = (dReal*) dAlloc (n*m*sizeof(dReal));
|
||||
memcpy (data,a.data,n*m*sizeof(dReal));
|
||||
}
|
||||
|
||||
|
||||
dMatrix::dMatrix (int rows, int cols,
|
||||
dReal *_data, int rowskip, int colskip)
|
||||
{
|
||||
if (rows < 1 || cols < 1) dDebug (0,"bad matrix size");
|
||||
n = rows;
|
||||
m = cols;
|
||||
data = (dReal*) dAlloc (n*m*sizeof(dReal));
|
||||
for (int i=0; i<n; i++) {
|
||||
for (int j=0; j<m; j++) data[i*m+j] = _data[i*rowskip + j*colskip];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
dMatrix::~dMatrix()
|
||||
{
|
||||
if (data) dFree (data,n*m*sizeof(dReal));
|
||||
}
|
||||
|
||||
|
||||
dReal & dMatrix::operator () (int i, int j)
|
||||
{
|
||||
if (i < 0 || i >= n || j < 0 || j >= m) dDebug (0,"bad matrix (i,j)");
|
||||
return data [i*m+j];
|
||||
}
|
||||
|
||||
|
||||
void dMatrix::operator= (const dMatrix &a)
|
||||
{
|
||||
if (data) dFree (data,n*m*sizeof(dReal));
|
||||
n = a.n;
|
||||
m = a.m;
|
||||
if (n > 0 && m > 0) {
|
||||
data = (dReal*) dAlloc (n*m*sizeof(dReal));
|
||||
memcpy (data,a.data,n*m*sizeof(dReal));
|
||||
}
|
||||
else data = 0;
|
||||
}
|
||||
|
||||
|
||||
void dMatrix::operator= (dReal a)
|
||||
{
|
||||
for (int i=0; i<n*m; i++) data[i] = a;
|
||||
}
|
||||
|
||||
|
||||
dMatrix dMatrix::transpose()
|
||||
{
|
||||
dMatrix r (m,n);
|
||||
for (int i=0; i<n; i++) {
|
||||
for (int j=0; j<m; j++) r.data[j*n+i] = data[i*m+j];
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
|
||||
dMatrix dMatrix::select (int np, int *p, int nq, int *q)
|
||||
{
|
||||
if (np < 1 || nq < 1) dDebug (0,"Matrix select, bad index array sizes");
|
||||
dMatrix r (np,nq);
|
||||
for (int i=0; i<np; i++) {
|
||||
for (int j=0; j<nq; j++) {
|
||||
if (p[i] < 0 || p[i] >= n || q[i] < 0 || q[i] >= m)
|
||||
dDebug (0,"Matrix select, bad index arrays");
|
||||
r.data[i*nq+j] = data[p[i]*m+q[j]];
|
||||
}
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
|
||||
dMatrix dMatrix::operator + (const dMatrix &a)
|
||||
{
|
||||
if (n != a.n || m != a.m) dDebug (0,"matrix +, mismatched sizes");
|
||||
dMatrix r (n,m);
|
||||
for (int i=0; i<n*m; i++) r.data[i] = data[i] + a.data[i];
|
||||
return r;
|
||||
}
|
||||
|
||||
|
||||
dMatrix dMatrix::operator - (const dMatrix &a)
|
||||
{
|
||||
if (n != a.n || m != a.m) dDebug (0,"matrix -, mismatched sizes");
|
||||
dMatrix r (n,m);
|
||||
for (int i=0; i<n*m; i++) r.data[i] = data[i] - a.data[i];
|
||||
return r;
|
||||
}
|
||||
|
||||
|
||||
dMatrix dMatrix::operator - ()
|
||||
{
|
||||
dMatrix r (n,m);
|
||||
for (int i=0; i<n*m; i++) r.data[i] = -data[i];
|
||||
return r;
|
||||
}
|
||||
|
||||
|
||||
dMatrix dMatrix::operator * (const dMatrix &a)
|
||||
{
|
||||
if (m != a.n) dDebug (0,"matrix *, mismatched sizes");
|
||||
dMatrix r (n,a.m);
|
||||
for (int i=0; i<n; i++) {
|
||||
for (int j=0; j<a.m; j++) {
|
||||
dReal sum = 0;
|
||||
for (int k=0; k<m; k++) sum += data[i*m+k] * a.data[k*a.m+j];
|
||||
r.data [i*a.m+j] = sum;
|
||||
}
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
|
||||
void dMatrix::operator += (const dMatrix &a)
|
||||
{
|
||||
if (n != a.n || m != a.m) dDebug (0,"matrix +=, mismatched sizes");
|
||||
for (int i=0; i<n*m; i++) data[i] += a.data[i];
|
||||
}
|
||||
|
||||
|
||||
void dMatrix::operator -= (const dMatrix &a)
|
||||
{
|
||||
if (n != a.n || m != a.m) dDebug (0,"matrix -=, mismatched sizes");
|
||||
for (int i=0; i<n*m; i++) data[i] -= a.data[i];
|
||||
}
|
||||
|
||||
|
||||
void dMatrix::clearUpperTriangle()
|
||||
{
|
||||
if (n != m) dDebug (0,"clearUpperTriangle() only works on square matrices");
|
||||
for (int i=0; i<n; i++) {
|
||||
for (int j=i+1; j<m; j++) data[i*m+j] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dMatrix::clearLowerTriangle()
|
||||
{
|
||||
if (n != m) dDebug (0,"clearLowerTriangle() only works on square matrices");
|
||||
for (int i=0; i<n; i++) {
|
||||
for (int j=0; j<i; j++) data[i*m+j] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dMatrix::makeRandom (dReal range)
|
||||
{
|
||||
for (int i=0; i<n; i++) {
|
||||
for (int j=0; j<m; j++)
|
||||
data[i*m+j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dMatrix::print (char *fmt, FILE *f)
|
||||
{
|
||||
for (int i=0; i<n; i++) {
|
||||
for (int j=0; j<m; j++) fprintf (f,fmt,data[i*m+j]);
|
||||
fprintf (f,"\n");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
dReal dMatrix::maxDifference (const dMatrix &a)
|
||||
{
|
||||
if (n != a.n || m != a.m) dDebug (0,"maxDifference(), mismatched sizes");
|
||||
dReal max = 0;
|
||||
for (int i=0; i<n; i++) {
|
||||
for (int j=0; j<m; j++) {
|
||||
dReal diff = dFabs(data[i*m+j] - a.data[i*m+j]);
|
||||
if (diff > max) max = diff;
|
||||
}
|
||||
}
|
||||
return max;
|
||||
}
|
||||
72
extern/ode/dist/ode/src/mat.h
vendored
72
extern/ode/dist/ode/src/mat.h
vendored
@@ -1,72 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
// matrix class. this is mostly for convenience in the testing code, it is
|
||||
// not optimized at all. correctness is much more importance here.
|
||||
|
||||
#ifndef _ODE_MAT_H_
|
||||
#define _ODE_MAT_H_
|
||||
|
||||
#include <ode/common.h>
|
||||
|
||||
|
||||
class dMatrix {
|
||||
int n,m; // matrix dimension, n,m >= 0
|
||||
dReal *data; // if nonzero, n*m elements allocated on the heap
|
||||
|
||||
public:
|
||||
// constructors, destructors
|
||||
dMatrix(); // make default 0x0 matrix
|
||||
dMatrix (int rows, int cols); // construct zero matrix of given size
|
||||
dMatrix (const dMatrix &); // construct copy of given matrix
|
||||
// create copy of given data - element (i,j) is data[i*rowskip+j*colskip]
|
||||
dMatrix (int rows, int cols, dReal *_data, int rowskip, int colskip);
|
||||
~dMatrix(); // destructor
|
||||
|
||||
// data movement
|
||||
dReal & operator () (int i, int j); // reference an element
|
||||
void operator= (const dMatrix &); // matrix = matrix
|
||||
void operator= (dReal); // matrix = scalar
|
||||
dMatrix transpose(); // return transposed matrix
|
||||
// return a permuted submatrix of this matrix, made up of the rows in p
|
||||
// and the columns in q. p has np elements, q has nq elements.
|
||||
dMatrix select (int np, int *p, int nq, int *q);
|
||||
|
||||
// operators
|
||||
dMatrix operator + (const dMatrix &);
|
||||
dMatrix operator - (const dMatrix &);
|
||||
dMatrix operator - ();
|
||||
dMatrix operator * (const dMatrix &);
|
||||
void operator += (const dMatrix &);
|
||||
void operator -= (const dMatrix &);
|
||||
|
||||
// utility
|
||||
void clearUpperTriangle();
|
||||
void clearLowerTriangle();
|
||||
void makeRandom (dReal range);
|
||||
void print (char *fmt = "%10.4f ", FILE *f=stdout);
|
||||
dReal maxDifference (const dMatrix &);
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
358
extern/ode/dist/ode/src/matrix.cpp
vendored
358
extern/ode/dist/ode/src/matrix.cpp
vendored
@@ -1,358 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#include <ode/common.h>
|
||||
#include <ode/matrix.h>
|
||||
|
||||
// misc defines
|
||||
#define ALLOCA dALLOCA16
|
||||
|
||||
|
||||
void dSetZero (dReal *a, int n)
|
||||
{
|
||||
dAASSERT (a && n >= 0);
|
||||
while (n > 0) {
|
||||
*(a++) = 0;
|
||||
n--;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dSetValue (dReal *a, int n, dReal value)
|
||||
{
|
||||
dAASSERT (a && n >= 0);
|
||||
while (n > 0) {
|
||||
*(a++) = value;
|
||||
n--;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dMultiply0 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)
|
||||
{
|
||||
int i,j,k,qskip,rskip,rpad;
|
||||
dAASSERT (A && B && C && p>0 && q>0 && r>0);
|
||||
qskip = dPAD(q);
|
||||
rskip = dPAD(r);
|
||||
rpad = rskip - r;
|
||||
dReal sum;
|
||||
const dReal *b,*c,*bb;
|
||||
bb = B;
|
||||
for (i=p; i; i--) {
|
||||
for (j=0 ; j<r; j++) {
|
||||
c = C + j;
|
||||
b = bb;
|
||||
sum = 0;
|
||||
for (k=q; k; k--, c+=rskip) sum += (*(b++))*(*c);
|
||||
*(A++) = sum;
|
||||
}
|
||||
A += rpad;
|
||||
bb += qskip;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dMultiply1 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)
|
||||
{
|
||||
int i,j,k,pskip,rskip;
|
||||
dReal sum;
|
||||
dAASSERT (A && B && C && p>0 && q>0 && r>0);
|
||||
pskip = dPAD(p);
|
||||
rskip = dPAD(r);
|
||||
for (i=0; i<p; i++) {
|
||||
for (j=0; j<r; j++) {
|
||||
sum = 0;
|
||||
for (k=0; k<q; k++) sum += B[i+k*pskip] * C[j+k*rskip];
|
||||
A[i*rskip+j] = sum;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dMultiply2 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)
|
||||
{
|
||||
int i,j,k,z,rpad,qskip;
|
||||
dReal sum;
|
||||
const dReal *bb,*cc;
|
||||
dAASSERT (A && B && C && p>0 && q>0 && r>0);
|
||||
rpad = dPAD(r) - r;
|
||||
qskip = dPAD(q);
|
||||
bb = B;
|
||||
for (i=p; i; i--) {
|
||||
cc = C;
|
||||
for (j=r; j; j--) {
|
||||
z = 0;
|
||||
sum = 0;
|
||||
for (k=q; k; k--,z++) sum += bb[z] * cc[z];
|
||||
*(A++) = sum;
|
||||
cc += qskip;
|
||||
}
|
||||
A += rpad;
|
||||
bb += qskip;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int dFactorCholesky (dReal *A, int n)
|
||||
{
|
||||
int i,j,k,nskip;
|
||||
dReal sum,*a,*b,*aa,*bb,*cc,*recip;
|
||||
dAASSERT (n > 0 && A);
|
||||
nskip = dPAD (n);
|
||||
recip = (dReal*) ALLOCA (n * sizeof(dReal));
|
||||
aa = A;
|
||||
for (i=0; i<n; i++) {
|
||||
bb = A;
|
||||
cc = A + i*nskip;
|
||||
for (j=0; j<i; j++) {
|
||||
sum = *cc;
|
||||
a = aa;
|
||||
b = bb;
|
||||
for (k=j; k; k--) sum -= (*(a++))*(*(b++));
|
||||
*cc = sum * recip[j];
|
||||
bb += nskip;
|
||||
cc++;
|
||||
}
|
||||
sum = *cc;
|
||||
a = aa;
|
||||
for (k=i; k; k--, a++) sum -= (*a)*(*a);
|
||||
if (sum <= REAL(0.0)) return 0;
|
||||
*cc = dSqrt(sum);
|
||||
recip[i] = dRecip (*cc);
|
||||
aa += nskip;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
void dSolveCholesky (const dReal *L, dReal *b, int n)
|
||||
{
|
||||
int i,k,nskip;
|
||||
dReal sum,*y;
|
||||
dAASSERT (n > 0 && L && b);
|
||||
nskip = dPAD (n);
|
||||
y = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
for (i=0; i<n; i++) {
|
||||
sum = 0;
|
||||
for (k=0; k < i; k++) sum += L[i*nskip+k]*y[k];
|
||||
y[i] = (b[i]-sum)/L[i*nskip+i];
|
||||
}
|
||||
for (i=n-1; i >= 0; i--) {
|
||||
sum = 0;
|
||||
for (k=i+1; k < n; k++) sum += L[k*nskip+i]*b[k];
|
||||
b[i] = (y[i]-sum)/L[i*nskip+i];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n)
|
||||
{
|
||||
int i,j,nskip;
|
||||
dReal *L,*x;
|
||||
dAASSERT (n > 0 && A && Ainv);
|
||||
nskip = dPAD (n);
|
||||
L = (dReal*) ALLOCA (nskip*n*sizeof(dReal));
|
||||
memcpy (L,A,nskip*n*sizeof(dReal));
|
||||
x = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
if (dFactorCholesky (L,n)==0) return 0;
|
||||
dSetZero (Ainv,n*nskip); // make sure all padding elements set to 0
|
||||
for (i=0; i<n; i++) {
|
||||
for (j=0; j<n; j++) x[j] = 0;
|
||||
x[i] = 1;
|
||||
dSolveCholesky (L,x,n);
|
||||
for (j=0; j<n; j++) Ainv[j*nskip+i] = x[j];
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
int dIsPositiveDefinite (const dReal *A, int n)
|
||||
{
|
||||
dReal *Acopy;
|
||||
dAASSERT (n > 0 && A);
|
||||
int nskip = dPAD (n);
|
||||
Acopy = (dReal*) ALLOCA (nskip*n * sizeof(dReal));
|
||||
memcpy (Acopy,A,nskip*n * sizeof(dReal));
|
||||
return dFactorCholesky (Acopy,n);
|
||||
}
|
||||
|
||||
|
||||
/***** this has been replaced by a faster version
|
||||
void dSolveL1T (const dReal *L, dReal *b, int n, int nskip)
|
||||
{
|
||||
int i,j;
|
||||
dAASSERT (L && b && n >= 0 && nskip >= n);
|
||||
dReal sum;
|
||||
for (i=n-2; i>=0; i--) {
|
||||
sum = 0;
|
||||
for (j=i+1; j<n; j++) sum += L[j*nskip+i]*b[j];
|
||||
b[i] -= sum;
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
void dVectorScale (dReal *a, const dReal *d, int n)
|
||||
{
|
||||
dAASSERT (a && d && n >= 0);
|
||||
for (int i=0; i<n; i++) a[i] *= d[i];
|
||||
}
|
||||
|
||||
|
||||
void dSolveLDLT (const dReal *L, const dReal *d, dReal *b, int n, int nskip)
|
||||
{
|
||||
dAASSERT (L && d && b && n > 0 && nskip >= n);
|
||||
dSolveL1 (L,b,n,nskip);
|
||||
dVectorScale (b,d,n);
|
||||
dSolveL1T (L,b,n,nskip);
|
||||
}
|
||||
|
||||
|
||||
void dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip)
|
||||
{
|
||||
int j,p;
|
||||
dReal *W1,*W2,W11,W21,alpha1,alpha2,alphanew,gamma1,gamma2,k1,k2,Wp,ell,dee;
|
||||
dAASSERT (L && d && a && n > 0 && nskip >= n);
|
||||
|
||||
if (n < 2) return;
|
||||
W1 = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
W2 = (dReal*) ALLOCA (n*sizeof(dReal));
|
||||
|
||||
W1[0] = 0;
|
||||
W2[0] = 0;
|
||||
for (j=1; j<n; j++) W1[j] = W2[j] = a[j] * M_SQRT1_2;
|
||||
W11 = (REAL(0.5)*a[0]+1)*M_SQRT1_2;
|
||||
W21 = (REAL(0.5)*a[0]-1)*M_SQRT1_2;
|
||||
|
||||
alpha1=1;
|
||||
alpha2=1;
|
||||
|
||||
dee = d[0];
|
||||
alphanew = alpha1 + (W11*W11)*dee;
|
||||
dee /= alphanew;
|
||||
gamma1 = W11 * dee;
|
||||
dee *= alpha1;
|
||||
alpha1 = alphanew;
|
||||
alphanew = alpha2 - (W21*W21)*dee;
|
||||
dee /= alphanew;
|
||||
gamma2 = W21 * dee;
|
||||
alpha2 = alphanew;
|
||||
k1 = REAL(1.0) - W21*gamma1;
|
||||
k2 = W21*gamma1*W11 - W21;
|
||||
for (p=1; p<n; p++) {
|
||||
Wp = W1[p];
|
||||
ell = L[p*nskip];
|
||||
W1[p] = Wp - W11*ell;
|
||||
W2[p] = k1*Wp + k2*ell;
|
||||
}
|
||||
|
||||
for (j=1; j<n; j++) {
|
||||
dee = d[j];
|
||||
alphanew = alpha1 + (W1[j]*W1[j])*dee;
|
||||
dee /= alphanew;
|
||||
gamma1 = W1[j] * dee;
|
||||
dee *= alpha1;
|
||||
alpha1 = alphanew;
|
||||
alphanew = alpha2 - (W2[j]*W2[j])*dee;
|
||||
dee /= alphanew;
|
||||
gamma2 = W2[j] * dee;
|
||||
dee *= alpha2;
|
||||
d[j] = dee;
|
||||
alpha2 = alphanew;
|
||||
|
||||
k1 = W1[j];
|
||||
k2 = W2[j];
|
||||
for (p=j+1; p<n; p++) {
|
||||
ell = L[p*nskip+j];
|
||||
Wp = W1[p] - k1 * ell;
|
||||
ell += gamma1 * Wp;
|
||||
W1[p] = Wp;
|
||||
Wp = W2[p] - k2 * ell;
|
||||
ell -= gamma2 * Wp;
|
||||
W2[p] = Wp;
|
||||
L[p*nskip+j] = ell;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// macros for dLDLTRemove() for accessing A - either access the matrix
|
||||
// directly or access it via row pointers. we are only supposed to reference
|
||||
// the lower triangle of A (it is symmetric), but indexes i and j come from
|
||||
// permutation vectors so they are not predictable. so do a test on the
|
||||
// indexes - this should not slow things down too much, as we don't do this
|
||||
// in an inner loop.
|
||||
|
||||
#define _GETA(i,j) (A[i][j])
|
||||
//#define _GETA(i,j) (A[(i)*nskip+(j)])
|
||||
#define GETA(i,j) ((i > j) ? _GETA(i,j) : _GETA(j,i))
|
||||
|
||||
|
||||
void dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d,
|
||||
int n1, int n2, int r, int nskip)
|
||||
{
|
||||
int i;
|
||||
dAASSERT(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 &&
|
||||
n1 >= n2 && nskip >= n1);
|
||||
#ifndef dNODEBUG
|
||||
for (i=0; i<n2; i++) dIASSERT(p[i] >= 0 && p[i] < n1);
|
||||
#endif
|
||||
|
||||
if (r==n2-1) {
|
||||
return; // deleting last row/col is easy
|
||||
}
|
||||
else if (r==0) {
|
||||
dReal *a = (dReal*) ALLOCA (n2 * sizeof(dReal));
|
||||
for (i=0; i<n2; i++) a[i] = -GETA(p[i],p[0]);
|
||||
a[0] += REAL(1.0);
|
||||
dLDLTAddTL (L,d,a,n2,nskip);
|
||||
}
|
||||
else {
|
||||
dReal *t = (dReal*) ALLOCA (r * sizeof(dReal));
|
||||
dReal *a = (dReal*) ALLOCA ((n2-r) * sizeof(dReal));
|
||||
for (i=0; i<r; i++) t[i] = L[r*nskip+i] / d[i];
|
||||
for (i=0; i<(n2-r); i++)
|
||||
a[i] = dDot(L+(r+i)*nskip,t,r) - GETA(p[r+i],p[r]);
|
||||
a[0] += REAL(1.0);
|
||||
dLDLTAddTL (L + r*nskip+r, d+r, a, n2-r, nskip);
|
||||
}
|
||||
|
||||
// snip out row/column r from L and d
|
||||
dRemoveRowCol (L,n2,nskip,r);
|
||||
if (r < (n2-1)) memmove (d+r,d+r+1,(n2-r-1)*sizeof(dReal));
|
||||
}
|
||||
|
||||
|
||||
void dRemoveRowCol (dReal *A, int n, int nskip, int r)
|
||||
{
|
||||
int i;
|
||||
dAASSERT(A && n > 0 && nskip >= n && r >= 0 && r < n);
|
||||
if (r >= n-1) return;
|
||||
if (r > 0) {
|
||||
for (i=0; i<r; i++)
|
||||
memmove (A+i*nskip+r,A+i*nskip+r+1,(n-r-1)*sizeof(dReal));
|
||||
for (i=r; i<(n-1); i++)
|
||||
memcpy (A+i*nskip,A+i*nskip+nskip,r*sizeof(dReal));
|
||||
}
|
||||
for (i=r; i<(n-1); i++)
|
||||
memcpy (A+i*nskip+r,A+i*nskip+nskip+r+1,(n-r-1)*sizeof(dReal));
|
||||
}
|
||||
278
extern/ode/dist/ode/src/memory.cpp
vendored
278
extern/ode/dist/ode/src/memory.cpp
vendored
@@ -1,278 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
// if the dDEBUG_ALLOC macro is defined, the following tests are performed in
|
||||
// the default allocator:
|
||||
// - size > 0 for alloc and realloc
|
||||
// - realloc and free operate on uncorrupted blocks
|
||||
// - realloc and free with the correct sizes
|
||||
// - on exit, report of block allocation statistics
|
||||
// - on exit, report of unfreed blocks and if they are corrupted
|
||||
// the allocator will also call Debug() when it allocates a block with
|
||||
// sequence number `_d_seqstop' or pointer value `_d_ptrstop'. these variables
|
||||
// are global and can be set in the debugger.
|
||||
|
||||
#include <ode/config.h>
|
||||
#include <ode/memory.h>
|
||||
#include <ode/error.h>
|
||||
|
||||
#ifdef dDEBUG_ALLOC
|
||||
|
||||
// when debugging, this is a header that it put at start of all blocks.
|
||||
// it contains `padding', which are PADSIZE elements of known value just
|
||||
// before the user memory starts. another PADSIZE padding elements are put
|
||||
// at the end of the user memory. the idea is that if the user accidentally
|
||||
// steps outside the allocated memory, it can hopefully be detected by
|
||||
// examining the padding elements.
|
||||
|
||||
#define PADSIZE 10
|
||||
struct blockHeader {
|
||||
long pad1; // protective padding
|
||||
long seq; // sequence number
|
||||
long size; // data size
|
||||
long flags; // bit 0 = ignore this block in final report
|
||||
blockHeader *next,*prev; // next & previous blocks
|
||||
long pad[PADSIZE]; // protective padding
|
||||
};
|
||||
|
||||
// compute the memory block size, including padding. the user memory block is
|
||||
// rounded up to a multiple of 4 bytes, to get alignment for the padding at
|
||||
// the end of the block.
|
||||
|
||||
#define BSIZE(size) (((((size)-1) | 3)+1) + sizeof(blockHeader) + \
|
||||
PADSIZE * sizeof(long))
|
||||
|
||||
static blockHeader dummyblock = {0,0,0,0,&dummyblock,&dummyblock,
|
||||
{0,0,0,0,0,0,0,0,0,0}};
|
||||
static blockHeader *firstblock = &dummyblock;
|
||||
static long num_blocks_alloced = 0;
|
||||
static long num_bytes_alloced = 0;
|
||||
static long total_num_blocks_alloced = 0;
|
||||
static long total_num_bytes_alloced = 0;
|
||||
static long max_blocks_alloced = 0;
|
||||
static long max_bytes_alloced = 0;
|
||||
|
||||
long _d_seqstop = 0;
|
||||
void *_d_ptrstop = 0;
|
||||
|
||||
static int checkBlockOk (void *ptr, int fatal)
|
||||
{
|
||||
if (ptr==0) dDebug (0,"0 passed to checkBlockOk()");
|
||||
blockHeader *b = ((blockHeader*) ptr) - 1;
|
||||
int i,ok = 1;
|
||||
if (b->pad1 != (long)0xdeadbeef || b->seq < 0 || b->size < 0) ok = 0;
|
||||
if (ok) {
|
||||
for (i=0; i<PADSIZE; i++) if (b->pad[i] != (long)0xdeadbeef) ok = 0;
|
||||
}
|
||||
if (ok) {
|
||||
long *endpad = (long*) (((char*)ptr) + (((b->size-1) | 3)+1));
|
||||
for (i=0; i<PADSIZE; i++) if (endpad[i] != (long)0xdeadbeef) ok = 0;
|
||||
}
|
||||
if (!ok && fatal)
|
||||
dDebug (0,"corrupted memory block found, ptr=%p, size=%d, "
|
||||
"seq=%ld", ptr,b->size,b->seq);
|
||||
return ok;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
static dAllocFunction *allocfn = 0;
|
||||
static dReallocFunction *reallocfn = 0;
|
||||
static dFreeFunction *freefn = 0;
|
||||
|
||||
|
||||
|
||||
void dSetAllocHandler (dAllocFunction *fn)
|
||||
{
|
||||
allocfn = fn;
|
||||
}
|
||||
|
||||
|
||||
void dSetReallocHandler (dReallocFunction *fn)
|
||||
{
|
||||
reallocfn = fn;
|
||||
}
|
||||
|
||||
|
||||
void dSetFreeHandler (dFreeFunction *fn)
|
||||
{
|
||||
freefn = fn;
|
||||
}
|
||||
|
||||
|
||||
dAllocFunction *dGetAllocHandler()
|
||||
{
|
||||
return allocfn;
|
||||
}
|
||||
|
||||
|
||||
dReallocFunction *dGetReallocHandler()
|
||||
{
|
||||
return reallocfn;
|
||||
}
|
||||
|
||||
|
||||
dFreeFunction *dGetFreeHandler()
|
||||
{
|
||||
return freefn;
|
||||
}
|
||||
|
||||
|
||||
void * dAlloc (int size)
|
||||
{
|
||||
#ifdef dDEBUG_ALLOC
|
||||
if (size < 1) dDebug (0,"bad block size to Alloc()");
|
||||
|
||||
num_blocks_alloced++;
|
||||
num_bytes_alloced += size;
|
||||
total_num_blocks_alloced++;
|
||||
total_num_bytes_alloced += size;
|
||||
if (num_blocks_alloced > max_blocks_alloced)
|
||||
max_blocks_alloced = num_blocks_alloced;
|
||||
if (num_bytes_alloced > max_bytes_alloced)
|
||||
max_bytes_alloced = num_bytes_alloced;
|
||||
|
||||
if (total_num_blocks_alloced == _d_seqstop)
|
||||
dDebug (0,"ALLOCATOR TRAP ON SEQUENCE NUMBER %d",_d_seqstop);
|
||||
long size2 = BSIZE(size);
|
||||
blockHeader *b = (blockHeader*) malloc (size2);
|
||||
if (b+1 == _d_ptrstop)
|
||||
dDebug (0,"ALLOCATOR TRAP ON BLOCK POINTER %p",_d_ptrstop);
|
||||
for (unsigned int i=0; i < (size2/sizeof(long)); i++)
|
||||
((long*)b)[i] = 0xdeadbeef;
|
||||
b->seq = total_num_blocks_alloced;
|
||||
b->size = size;
|
||||
b->flags = 0;
|
||||
b->next = firstblock->next;
|
||||
b->prev = firstblock;
|
||||
firstblock->next->prev = b;
|
||||
firstblock->next = b;
|
||||
return b + 1;
|
||||
#else
|
||||
if (allocfn) return allocfn (size); else return malloc (size);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
void * dRealloc (void *ptr, int oldsize, int newsize)
|
||||
{
|
||||
#ifdef dDEBUG_ALLOC
|
||||
if (ptr==0) dDebug (0,"Realloc() called with ptr==0");
|
||||
checkBlockOk (ptr,1);
|
||||
blockHeader *b = ((blockHeader*) ptr) - 1;
|
||||
if (b->size != oldsize)
|
||||
dDebug (0,"Realloc(%p,%d,%d) has bad old size, good size "
|
||||
"is %ld, seq=%ld",ptr,oldsize,newsize,b->size,b->seq);
|
||||
void *p = dAlloc (newsize);
|
||||
blockHeader *newb = ((blockHeader*) p) - 1;
|
||||
newb->flags = b->flags;
|
||||
if (oldsize>=1) memcpy (p,ptr,oldsize);
|
||||
dFree (ptr,oldsize);
|
||||
return p;
|
||||
#else
|
||||
if (reallocfn) return reallocfn (ptr,oldsize,newsize);
|
||||
else return realloc (ptr,newsize);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
void dFree (void *ptr, int size)
|
||||
{
|
||||
if (!ptr) return;
|
||||
#ifdef dDEBUG_ALLOC
|
||||
checkBlockOk (ptr,1);
|
||||
blockHeader *b = ((blockHeader*) ptr) - 1;
|
||||
if (b->size != size)
|
||||
dDebug (0,"Free(%p,%d) has bad size, good size "
|
||||
"is %ld, seq=%ld",ptr,size,b->size,b->seq);
|
||||
num_blocks_alloced--;
|
||||
num_bytes_alloced -= b->size;
|
||||
if (num_blocks_alloced < 0 || num_bytes_alloced < 0)
|
||||
dDebug (0,"Free called too many times");
|
||||
|
||||
b->next->prev = b->prev;
|
||||
b->prev->next = b->next;
|
||||
memset (b,0,BSIZE(b->size));
|
||||
|
||||
free (b);
|
||||
#else
|
||||
if (freefn) freefn (ptr,size); else free (ptr);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
void dAllocDontReport (void *ptr)
|
||||
{
|
||||
#ifdef dDEBUG_ALLOC
|
||||
checkBlockOk (ptr,1);
|
||||
blockHeader *b = ((blockHeader*) ptr) - 1;
|
||||
b->flags |= 1;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
#ifdef dDEBUG_ALLOC
|
||||
|
||||
static void printReport()
|
||||
{
|
||||
// subtract the "dont report" blocks from the totals
|
||||
blockHeader *b = firstblock;
|
||||
do {
|
||||
if (b != &dummyblock && (b->flags & 1)) {
|
||||
num_blocks_alloced--;
|
||||
num_bytes_alloced -= b->size;
|
||||
if (!checkBlockOk (b+1,0))
|
||||
fprintf (stderr,"\tCORRUPTED: %p, size=%ld, seq=%ld\n",b+1,
|
||||
b->size,b->seq);
|
||||
}
|
||||
b = b->prev;
|
||||
}
|
||||
while (b != firstblock);
|
||||
|
||||
fprintf (stderr,"\nALLOCATOR REPORT\n");
|
||||
fprintf (stderr,"\tblocks still allocated: %ld\n",num_blocks_alloced);
|
||||
fprintf (stderr,"\tbytes still allocated: %ld\n",num_bytes_alloced);
|
||||
fprintf (stderr,"\ttotal blocks allocated: %ld\n",total_num_blocks_alloced);
|
||||
fprintf (stderr,"\ttotal bytes allocated: %ld\n",total_num_bytes_alloced);
|
||||
fprintf (stderr,"\tmax blocks allocated: %ld\n",max_blocks_alloced);
|
||||
fprintf (stderr,"\tmax bytes allocated: %ld\n",max_bytes_alloced);
|
||||
|
||||
b = firstblock;
|
||||
do {
|
||||
if (b != &dummyblock && (b->flags & 1)==0) {
|
||||
int ok = checkBlockOk (b+1,0);
|
||||
fprintf (stderr,"\tUNFREED: %p, size=%ld, seq=%ld (%s)\n",b+1,
|
||||
b->size,b->seq, ok ? "ok" : "CORUPTED");
|
||||
}
|
||||
b = b->prev;
|
||||
}
|
||||
while (b != firstblock);
|
||||
fprintf (stderr,"\n");
|
||||
}
|
||||
|
||||
|
||||
struct dMemoryReportPrinter {
|
||||
~dMemoryReportPrinter() { printReport(); }
|
||||
} _dReportPrinter;
|
||||
|
||||
#endif
|
||||
147
extern/ode/dist/ode/src/misc.cpp
vendored
147
extern/ode/dist/ode/src/misc.cpp
vendored
@@ -1,147 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#include <ode/config.h>
|
||||
#include <ode/misc.h>
|
||||
#include <ode/matrix.h>
|
||||
|
||||
//****************************************************************************
|
||||
// random numbers
|
||||
|
||||
static unsigned long seed = 0;
|
||||
|
||||
unsigned long dRand()
|
||||
{
|
||||
seed = (1664525L*seed + 1013904223L) & 0xffffffff;
|
||||
return seed;
|
||||
}
|
||||
|
||||
|
||||
unsigned long dRandGetSeed()
|
||||
{
|
||||
return seed;
|
||||
}
|
||||
|
||||
|
||||
void dRandSetSeed (unsigned long s)
|
||||
{
|
||||
seed = s;
|
||||
}
|
||||
|
||||
|
||||
int dTestRand()
|
||||
{
|
||||
unsigned long oldseed = seed;
|
||||
int ret = 1;
|
||||
seed = 0;
|
||||
if (dRand() != 0x3c6ef35f || dRand() != 0x47502932 ||
|
||||
dRand() != 0xd1ccf6e9 || dRand() != 0xaaf95334 ||
|
||||
dRand() != 0x6252e503) ret = 0;
|
||||
seed = oldseed;
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
int dRandInt (int n)
|
||||
{
|
||||
double a = double(n) / 4294967296.0;
|
||||
return (int) (double(dRand()) * a);
|
||||
}
|
||||
|
||||
|
||||
dReal dRandReal()
|
||||
{
|
||||
return ((dReal) dRand()) / ((dReal) 0xffffffff);
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// matrix utility stuff
|
||||
|
||||
void dPrintMatrix (dReal *A, int n, int m, char *fmt, FILE *f)
|
||||
{
|
||||
int i,j;
|
||||
int skip = dPAD(m);
|
||||
for (i=0; i<n; i++) {
|
||||
for (j=0; j<m; j++) fprintf (f,fmt,A[i*skip+j]);
|
||||
fprintf (f,"\n");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dMakeRandomVector (dReal *A, int n, dReal range)
|
||||
{
|
||||
int i;
|
||||
for (i=0; i<n; i++) A[i] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
|
||||
}
|
||||
|
||||
|
||||
void dMakeRandomMatrix (dReal *A, int n, int m, dReal range)
|
||||
{
|
||||
int i,j;
|
||||
int skip = dPAD(m);
|
||||
dSetZero (A,n*skip);
|
||||
for (i=0; i<n; i++) {
|
||||
for (j=0; j<m; j++) A[i*skip+j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dClearUpperTriangle (dReal *A, int n)
|
||||
{
|
||||
int i,j;
|
||||
int skip = dPAD(n);
|
||||
for (i=0; i<n; i++) {
|
||||
for (j=i+1; j<n; j++) A[i*skip+j] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
dReal dMaxDifference (const dReal *A, const dReal *B, int n, int m)
|
||||
{
|
||||
int i,j;
|
||||
int skip = dPAD(m);
|
||||
dReal diff,max;
|
||||
max = 0;
|
||||
for (i=0; i<n; i++) {
|
||||
for (j=0; j<m; j++) {
|
||||
diff = dFabs(A[i*skip+j] - B[i*skip+j]);
|
||||
if (diff > max) max = diff;
|
||||
}
|
||||
}
|
||||
return max;
|
||||
}
|
||||
|
||||
|
||||
dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n)
|
||||
{
|
||||
int i,j;
|
||||
int skip = dPAD(n);
|
||||
dReal diff,max;
|
||||
max = 0;
|
||||
for (i=0; i<n; i++) {
|
||||
for (j=0; j<=i; j++) {
|
||||
diff = dFabs(A[i*skip+j] - B[i*skip+j]);
|
||||
if (diff > max) max = diff;
|
||||
}
|
||||
}
|
||||
return max;
|
||||
}
|
||||
91
extern/ode/dist/ode/src/objects.h
vendored
91
extern/ode/dist/ode/src/objects.h
vendored
@@ -1,91 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
// object, body, and world structs.
|
||||
|
||||
|
||||
#ifndef _ODE_OBJECT_H_
|
||||
#define _ODE_OBJECT_H_
|
||||
|
||||
#include <ode/common.h>
|
||||
#include <ode/memory.h>
|
||||
#include <ode/mass.h>
|
||||
#include "array.h"
|
||||
|
||||
|
||||
// some body flags
|
||||
|
||||
enum {
|
||||
dxBodyFlagFiniteRotation = 1, // use finite rotations
|
||||
dxBodyFlagFiniteRotationAxis = 2, // use finite rotations only along axis
|
||||
dxBodyDisabled = 4, // body is disabled
|
||||
dxBodyNoGravity = 8 // body is not influenced by gravity
|
||||
};
|
||||
|
||||
|
||||
// base class that does correct object allocation / deallocation
|
||||
|
||||
struct dBase {
|
||||
void *operator new (size_t size) { return dAlloc (size); }
|
||||
void operator delete (void *ptr, size_t size) { dFree (ptr,size); }
|
||||
void *operator new[] (size_t size) { return dAlloc (size); }
|
||||
void operator delete[] (void *ptr, size_t size) { dFree (ptr,size); }
|
||||
};
|
||||
|
||||
|
||||
// base class for bodies and joints
|
||||
|
||||
struct dObject : public dBase {
|
||||
dxWorld *world; // world this object is in
|
||||
dObject *next; // next object of this type in list
|
||||
dObject **tome; // pointer to previous object's next ptr
|
||||
void *userdata; // user settable data
|
||||
int tag; // used by dynamics algorithms
|
||||
};
|
||||
|
||||
|
||||
struct dxBody : public dObject {
|
||||
dxJointNode *firstjoint; // list of attached joints
|
||||
int flags; // some dxBodyFlagXXX flags
|
||||
dMass mass; // mass parameters about POR
|
||||
dMatrix3 invI; // inverse of mass.I
|
||||
dReal invMass; // 1 / mass.mass
|
||||
dVector3 pos; // position of POR (point of reference)
|
||||
dQuaternion q; // orientation quaternion
|
||||
dMatrix3 R; // rotation matrix, always corresponds to q
|
||||
dVector3 lvel,avel; // linear and angular velocity of POR
|
||||
dVector3 facc,tacc; // force and torque accululators
|
||||
dVector3 finite_rot_axis; // finite rotation axis, unit length or 0=none
|
||||
};
|
||||
|
||||
|
||||
struct dxWorld : public dBase {
|
||||
dxBody *firstbody; // body linked list
|
||||
dxJoint *firstjoint; // joint linked list
|
||||
int nb,nj; // number of bodies and joints in lists
|
||||
dVector3 gravity; // gravity vector (m/s/s)
|
||||
dReal global_erp; // global error reduction parameter
|
||||
dReal global_cfm; // global costraint force mixing parameter
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
130
extern/ode/dist/ode/src/obstack.cpp
vendored
130
extern/ode/dist/ode/src/obstack.cpp
vendored
@@ -1,130 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#include <ode/common.h>
|
||||
#include <ode/error.h>
|
||||
#include <ode/memory.h>
|
||||
#include "obstack.h"
|
||||
|
||||
//****************************************************************************
|
||||
// macros and constants
|
||||
|
||||
#define ROUND_UP_OFFSET_TO_EFFICIENT_SIZE(arena,ofs) \
|
||||
ofs = (int) (dEFFICIENT_SIZE( ((intP)(arena)) + ofs ) - ((intP)(arena)) );
|
||||
|
||||
#define MAX_ALLOC_SIZE \
|
||||
((int)(dOBSTACK_ARENA_SIZE - sizeof (Arena) - EFFICIENT_ALIGNMENT + 1))
|
||||
|
||||
//****************************************************************************
|
||||
// dObStack
|
||||
|
||||
dObStack::dObStack()
|
||||
{
|
||||
first = 0;
|
||||
last = 0;
|
||||
current_arena = 0;
|
||||
current_ofs = 0;
|
||||
}
|
||||
|
||||
|
||||
dObStack::~dObStack()
|
||||
{
|
||||
// free all arenas
|
||||
Arena *a,*nexta;
|
||||
a = first;
|
||||
while (a) {
|
||||
nexta = a->next;
|
||||
dFree (a,dOBSTACK_ARENA_SIZE);
|
||||
a = nexta;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void *dObStack::alloc (int num_bytes)
|
||||
{
|
||||
if (num_bytes > MAX_ALLOC_SIZE) dDebug (0,"num_bytes too large");
|
||||
|
||||
// allocate or move to a new arena if necessary
|
||||
if (!first) {
|
||||
// allocate the first arena if necessary
|
||||
first = last = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE);
|
||||
first->next = 0;
|
||||
first->used = sizeof (Arena);
|
||||
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (first,first->used);
|
||||
}
|
||||
else {
|
||||
// we already have one or more arenas, see if a new arena must be used
|
||||
if ((last->used + num_bytes) > dOBSTACK_ARENA_SIZE) {
|
||||
if (!last->next) {
|
||||
last->next = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE);
|
||||
last->next->next = 0;
|
||||
}
|
||||
last = last->next;
|
||||
last->used = sizeof (Arena);
|
||||
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (last,last->used);
|
||||
}
|
||||
}
|
||||
|
||||
// allocate an area in the arena
|
||||
char *c = ((char*) last) + last->used;
|
||||
last->used += num_bytes;
|
||||
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (last,last->used);
|
||||
return c;
|
||||
}
|
||||
|
||||
|
||||
void dObStack::freeAll()
|
||||
{
|
||||
last = first;
|
||||
if (first) {
|
||||
first->used = sizeof(Arena);
|
||||
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (first,first->used);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void *dObStack::rewind()
|
||||
{
|
||||
current_arena = first;
|
||||
current_ofs = sizeof (Arena);
|
||||
if (current_arena) {
|
||||
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs)
|
||||
return ((char*) current_arena) + current_ofs;
|
||||
}
|
||||
else return 0;
|
||||
}
|
||||
|
||||
|
||||
void *dObStack::next (int num_bytes)
|
||||
{
|
||||
// this functions like alloc, except that no new storage is ever allocated
|
||||
if (!current_arena) return 0;
|
||||
current_ofs += num_bytes;
|
||||
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs);
|
||||
if (current_ofs >= current_arena->used) {
|
||||
current_arena = current_arena->next;
|
||||
if (!current_arena) return 0;
|
||||
current_ofs = sizeof (Arena);
|
||||
ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs);
|
||||
}
|
||||
return ((char*) current_arena) + current_ofs;
|
||||
}
|
||||
69
extern/ode/dist/ode/src/obstack.h
vendored
69
extern/ode/dist/ode/src/obstack.h
vendored
@@ -1,69 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef _ODE_OBSTACK_H_
|
||||
#define _ODE_OBSTACK_H_
|
||||
|
||||
#include "objects.h"
|
||||
|
||||
// each obstack Arena pointer points to a block of this many bytes
|
||||
#define dOBSTACK_ARENA_SIZE 16384
|
||||
|
||||
|
||||
struct dObStack : public dBase {
|
||||
struct Arena {
|
||||
Arena *next; // next arena in linked list
|
||||
int used; // total number of bytes used in this arena, counting
|
||||
}; // this header
|
||||
|
||||
Arena *first; // head of the arena linked list. 0 if no arenas yet
|
||||
Arena *last; // arena where blocks are currently being allocated
|
||||
|
||||
// used for iterator
|
||||
Arena *current_arena;
|
||||
int current_ofs;
|
||||
|
||||
dObStack();
|
||||
~dObStack();
|
||||
|
||||
void *alloc (int num_bytes);
|
||||
// allocate a block in the last arena, allocating a new arena if necessary.
|
||||
// it is a runtime error if num_bytes is larger than the arena size.
|
||||
|
||||
void freeAll();
|
||||
// free all blocks in all arenas. this does not deallocate the arenas
|
||||
// themselves, so future alloc()s will reuse them.
|
||||
|
||||
void *rewind();
|
||||
// rewind the obstack iterator, and return the address of the first
|
||||
// allocated block. return 0 if there are no allocated blocks.
|
||||
|
||||
void *next (int num_bytes);
|
||||
// return the address of the next allocated block. 'num_bytes' is the size
|
||||
// of the previous block. this returns null if there are no more arenas.
|
||||
// the sequence of 'num_bytes' parameters passed to next() during a
|
||||
// traversal of the list must exactly match the parameters passed to alloc().
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
1341
extern/ode/dist/ode/src/ode.cpp
vendored
1341
extern/ode/dist/ode/src/ode.cpp
vendored
@@ -1,1341 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
// this source file is mostly concerned with the data structures, not the
|
||||
// numerics.
|
||||
|
||||
#include "objects.h"
|
||||
#include <ode/ode.h>
|
||||
#include "joint.h"
|
||||
#include <ode/odemath.h>
|
||||
#include <ode/matrix.h>
|
||||
#include "step.h"
|
||||
#include <ode/memory.h>
|
||||
#include <ode/error.h>
|
||||
|
||||
// misc defines
|
||||
#define ALLOCA dALLOCA16
|
||||
|
||||
//****************************************************************************
|
||||
// utility
|
||||
|
||||
static inline void initObject (dObject *obj, dxWorld *w)
|
||||
{
|
||||
obj->world = w;
|
||||
obj->next = 0;
|
||||
obj->tome = 0;
|
||||
obj->userdata = 0;
|
||||
obj->tag = 0;
|
||||
}
|
||||
|
||||
|
||||
// add an object `obj' to the list who's head pointer is pointed to by `first'.
|
||||
|
||||
static inline void addObjectToList (dObject *obj, dObject **first)
|
||||
{
|
||||
obj->next = *first;
|
||||
obj->tome = first;
|
||||
if (*first) (*first)->tome = &obj->next;
|
||||
(*first) = obj;
|
||||
}
|
||||
|
||||
|
||||
// remove the object from the linked list
|
||||
|
||||
static inline void removeObjectFromList (dObject *obj)
|
||||
{
|
||||
if (obj->next) obj->next->tome = obj->tome;
|
||||
*(obj->tome) = obj->next;
|
||||
// safeguard
|
||||
obj->next = 0;
|
||||
obj->tome = 0;
|
||||
}
|
||||
|
||||
|
||||
// remove the joint from neighbour lists of all connected bodies
|
||||
|
||||
static void removeJointReferencesFromAttachedBodies (dxJoint *j)
|
||||
{
|
||||
for (int i=0; i<2; i++) {
|
||||
dxBody *body = j->node[i].body;
|
||||
if (body) {
|
||||
dxJointNode *n = body->firstjoint;
|
||||
dxJointNode *last = 0;
|
||||
while (n) {
|
||||
if (n->joint == j) {
|
||||
if (last) last->next = n->next;
|
||||
else body->firstjoint = n->next;
|
||||
break;
|
||||
}
|
||||
last = n;
|
||||
n = n->next;
|
||||
}
|
||||
}
|
||||
}
|
||||
j->node[0].body = 0;
|
||||
j->node[0].next = 0;
|
||||
j->node[1].body = 0;
|
||||
j->node[1].next = 0;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// island processing
|
||||
|
||||
// this groups all joints and bodies in a world into islands. all objects
|
||||
// in an island are reachable by going through connected bodies and joints.
|
||||
// each island can be simulated separately.
|
||||
// note that joints that are not attached to anything will not be included
|
||||
// in any island, an so they do not affect the simulation.
|
||||
//
|
||||
// this function starts new island from unvisited bodies. however, it will
|
||||
// never start a new islands from a disabled body. thus islands of disabled
|
||||
// bodies will not be included in the simulation. disabled bodies are
|
||||
// re-enabled if they are found to be part of an active island.
|
||||
|
||||
static void processIslands (dxWorld *world, dReal stepsize)
|
||||
{
|
||||
dxBody *b,*bb,**body;
|
||||
dxJoint *j,**joint;
|
||||
|
||||
// nothing to do if no bodies
|
||||
if (world->nb <= 0) return;
|
||||
|
||||
// make arrays for body and joint lists (for a single island) to go into
|
||||
body = (dxBody**) ALLOCA (world->nb * sizeof(dxBody*));
|
||||
joint = (dxJoint**) ALLOCA (world->nj * sizeof(dxJoint*));
|
||||
int bcount = 0; // number of bodies in `body'
|
||||
int jcount = 0; // number of joints in `joint'
|
||||
|
||||
// set all body/joint tags to 0
|
||||
for (b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0;
|
||||
for (j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0;
|
||||
|
||||
// allocate a stack of unvisited bodies in the island. the maximum size of
|
||||
// the stack can be the lesser of the number of bodies or joints, because
|
||||
// new bodies are only ever added to the stack by going through untagged
|
||||
// joints. all the bodies in the stack must be tagged!
|
||||
int stackalloc = (world->nj < world->nb) ? world->nj : world->nb;
|
||||
dxBody **stack = (dxBody**) ALLOCA (stackalloc * sizeof(dxBody*));
|
||||
|
||||
for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) {
|
||||
// get bb = the next enabled, untagged body, and tag it
|
||||
if (bb->tag || (bb->flags & dxBodyDisabled)) continue;
|
||||
bb->tag = 1;
|
||||
|
||||
// tag all bodies and joints starting from bb.
|
||||
int stacksize = 0;
|
||||
b = bb;
|
||||
body[0] = bb;
|
||||
bcount = 1;
|
||||
jcount = 0;
|
||||
goto quickstart;
|
||||
while (stacksize > 0) {
|
||||
b = stack[--stacksize]; // pop body off stack
|
||||
body[bcount++] = b; // put body on body list
|
||||
quickstart:
|
||||
|
||||
// traverse and tag all body's joints, add untagged connected bodies
|
||||
// to stack
|
||||
for (dxJointNode *n=b->firstjoint; n; n=n->next) {
|
||||
if (!n->joint->tag) {
|
||||
n->joint->tag = 1;
|
||||
joint[jcount++] = n->joint;
|
||||
if (n->body && !n->body->tag) {
|
||||
n->body->tag = 1;
|
||||
stack[stacksize++] = n->body;
|
||||
}
|
||||
}
|
||||
}
|
||||
dIASSERT(stacksize <= world->nb);
|
||||
dIASSERT(stacksize <= world->nj);
|
||||
}
|
||||
|
||||
// now do something with body and joint lists
|
||||
dInternalStepIsland (world,body,bcount,joint,jcount,stepsize);
|
||||
|
||||
// what we've just done may have altered the body/joint tag values.
|
||||
// we must make sure that these tags are nonzero.
|
||||
// also make sure all bodies are in the enabled state.
|
||||
int i;
|
||||
for (i=0; i<bcount; i++) {
|
||||
body[i]->tag = 1;
|
||||
body[i]->flags &= ~dxBodyDisabled;
|
||||
}
|
||||
for (i=0; i<jcount; i++) joint[i]->tag = 1;
|
||||
}
|
||||
|
||||
// if debugging, check that all objects (except for disabled bodies,
|
||||
// unconnected joints, and joints that are connected to disabled bodies)
|
||||
// were tagged.
|
||||
# ifndef dNODEBUG
|
||||
for (b=world->firstbody; b; b=(dxBody*)b->next) {
|
||||
if (b->flags & dxBodyDisabled) {
|
||||
if (b->tag) dDebug (0,"disabled body tagged");
|
||||
}
|
||||
else {
|
||||
if (!b->tag) dDebug (0,"enabled body not tagged");
|
||||
}
|
||||
}
|
||||
for (j=world->firstjoint; j; j=(dxJoint*)j->next) {
|
||||
if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled)==0) ||
|
||||
(j->node[1].body && (j->node[1].body->flags & dxBodyDisabled)==0)) {
|
||||
if (!j->tag) dDebug (0,"attached enabled joint not tagged");
|
||||
}
|
||||
else {
|
||||
if (j->tag) dDebug (0,"unattached or disabled joint tagged");
|
||||
}
|
||||
}
|
||||
# endif
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// debugging
|
||||
|
||||
// see if an object list loops on itself (if so, it's bad).
|
||||
|
||||
static int listHasLoops (dObject *first)
|
||||
{
|
||||
if (first==0 || first->next==0) return 0;
|
||||
dObject *a=first,*b=first->next;
|
||||
int skip=0;
|
||||
while (b) {
|
||||
if (a==b) return 1;
|
||||
b = b->next;
|
||||
if (skip) a = a->next;
|
||||
skip ^= 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
// check the validity of the world data structures
|
||||
|
||||
static void checkWorld (dxWorld *w)
|
||||
{
|
||||
dxBody *b;
|
||||
dxJoint *j;
|
||||
|
||||
// check there are no loops
|
||||
if (listHasLoops (w->firstbody)) dDebug (0,"body list has loops");
|
||||
if (listHasLoops (w->firstjoint)) dDebug (0,"joint list has loops");
|
||||
|
||||
// check lists are well formed (check `tome' pointers)
|
||||
for (b=w->firstbody; b; b=(dxBody*)b->next) {
|
||||
if (b->next && b->next->tome != &b->next)
|
||||
dDebug (0,"bad tome pointer in body list");
|
||||
}
|
||||
for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
|
||||
if (j->next && j->next->tome != &j->next)
|
||||
dDebug (0,"bad tome pointer in joint list");
|
||||
}
|
||||
|
||||
// check counts
|
||||
int n = 0;
|
||||
for (b=w->firstbody; b; b=(dxBody*)b->next) n++;
|
||||
if (w->nb != n) dDebug (0,"body count incorrect");
|
||||
n = 0;
|
||||
for (j=w->firstjoint; j; j=(dxJoint*)j->next) n++;
|
||||
if (w->nj != n) dDebug (0,"joint count incorrect");
|
||||
|
||||
// set all tag values to a known value
|
||||
static int count = 0;
|
||||
count++;
|
||||
for (b=w->firstbody; b; b=(dxBody*)b->next) b->tag = count;
|
||||
for (j=w->firstjoint; j; j=(dxJoint*)j->next) j->tag = count;
|
||||
|
||||
// check all body/joint world pointers are ok
|
||||
for (b=w->firstbody; b; b=(dxBody*)b->next) if (b->world != w)
|
||||
dDebug (0,"bad world pointer in body list");
|
||||
for (j=w->firstjoint; j; j=(dxJoint*)j->next) if (j->world != w)
|
||||
dDebug (0,"bad world pointer in joint list");
|
||||
|
||||
/*
|
||||
// check for half-connected joints - actually now these are valid
|
||||
for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
|
||||
if (j->node[0].body || j->node[1].body) {
|
||||
if (!(j->node[0].body && j->node[1].body))
|
||||
dDebug (0,"half connected joint found");
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
// check that every joint node appears in the joint lists of both bodies it
|
||||
// attaches
|
||||
for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
|
||||
for (int i=0; i<2; i++) {
|
||||
if (j->node[i].body) {
|
||||
int ok = 0;
|
||||
for (dxJointNode *n=j->node[i].body->firstjoint; n; n=n->next) {
|
||||
if (n->joint == j) ok = 1;
|
||||
}
|
||||
if (ok==0) dDebug (0,"joint not in joint list of attached body");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// check all body joint lists (correct body ptrs)
|
||||
for (b=w->firstbody; b; b=(dxBody*)b->next) {
|
||||
for (dxJointNode *n=b->firstjoint; n; n=n->next) {
|
||||
if (&n->joint->node[0] == n) {
|
||||
if (n->joint->node[1].body != b)
|
||||
dDebug (0,"bad body pointer in joint node of body list (1)");
|
||||
}
|
||||
else {
|
||||
if (n->joint->node[0].body != b)
|
||||
dDebug (0,"bad body pointer in joint node of body list (2)");
|
||||
}
|
||||
if (n->joint->tag != count) dDebug (0,"bad joint node pointer in body");
|
||||
}
|
||||
}
|
||||
|
||||
// check all body pointers in joints, check they are distinct
|
||||
for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
|
||||
if (j->node[0].body && (j->node[0].body == j->node[1].body))
|
||||
dDebug (0,"non-distinct body pointers in joint");
|
||||
if ((j->node[0].body && j->node[0].body->tag != count) ||
|
||||
(j->node[1].body && j->node[1].body->tag != count))
|
||||
dDebug (0,"bad body pointer in joint");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dWorldCheck (dxWorld *w)
|
||||
{
|
||||
checkWorld (w);
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// body
|
||||
|
||||
dxBody *dBodyCreate (dxWorld *w)
|
||||
{
|
||||
dAASSERT (w);
|
||||
dxBody *b = new dxBody;
|
||||
initObject (b,w);
|
||||
b->firstjoint = 0;
|
||||
b->flags = 0;
|
||||
dMassSetParameters (&b->mass,1,0,0,0,1,1,1,0,0,0);
|
||||
dSetZero (b->invI,4*3);
|
||||
b->invI[0] = 1;
|
||||
b->invI[5] = 1;
|
||||
b->invI[10] = 1;
|
||||
b->invMass = 1;
|
||||
dSetZero (b->pos,4);
|
||||
dSetZero (b->q,4);
|
||||
b->q[0] = 1;
|
||||
dRSetIdentity (b->R);
|
||||
dSetZero (b->lvel,4);
|
||||
dSetZero (b->avel,4);
|
||||
dSetZero (b->facc,4);
|
||||
dSetZero (b->tacc,4);
|
||||
dSetZero (b->finite_rot_axis,4);
|
||||
addObjectToList (b,(dObject **) &w->firstbody);
|
||||
w->nb++;
|
||||
return b;
|
||||
}
|
||||
|
||||
|
||||
void dBodyDestroy (dxBody *b)
|
||||
{
|
||||
dAASSERT (b);
|
||||
|
||||
// detach all neighbouring joints, then delete this body.
|
||||
dxJointNode *n = b->firstjoint;
|
||||
while (n) {
|
||||
// sneaky trick to speed up removal of joint references (black magic)
|
||||
n->joint->node[(n == n->joint->node)].body = 0;
|
||||
|
||||
dxJointNode *next = n->next;
|
||||
n->next = 0;
|
||||
removeJointReferencesFromAttachedBodies (n->joint);
|
||||
n = next;
|
||||
}
|
||||
removeObjectFromList (b);
|
||||
b->world->nb--;
|
||||
delete b;
|
||||
}
|
||||
|
||||
|
||||
void dBodySetData (dBodyID b, void *data)
|
||||
{
|
||||
dAASSERT (b);
|
||||
b->userdata = data;
|
||||
}
|
||||
|
||||
|
||||
void *dBodyGetData (dBodyID b)
|
||||
{
|
||||
dAASSERT (b);
|
||||
return b->userdata;
|
||||
}
|
||||
|
||||
|
||||
void dBodySetPosition (dBodyID b, dReal x, dReal y, dReal z)
|
||||
{
|
||||
dAASSERT (b);
|
||||
b->pos[0] = x;
|
||||
b->pos[1] = y;
|
||||
b->pos[2] = z;
|
||||
}
|
||||
|
||||
|
||||
void dBodySetRotation (dBodyID b, const dMatrix3 R)
|
||||
{
|
||||
dAASSERT (b && R);
|
||||
dQuaternion q;
|
||||
dRtoQ (R,q);
|
||||
dNormalize4 (q);
|
||||
b->q[0] = q[0];
|
||||
b->q[1] = q[1];
|
||||
b->q[2] = q[2];
|
||||
b->q[3] = q[3];
|
||||
dQtoR (b->q,b->R);
|
||||
}
|
||||
|
||||
|
||||
void dBodySetQuaternion (dBodyID b, const dQuaternion q)
|
||||
{
|
||||
dAASSERT (b && q);
|
||||
b->q[0] = q[0];
|
||||
b->q[1] = q[1];
|
||||
b->q[2] = q[2];
|
||||
b->q[3] = q[3];
|
||||
dNormalize4 (b->q);
|
||||
dQtoR (b->q,b->R);
|
||||
}
|
||||
|
||||
|
||||
void dBodySetLinearVel (dBodyID b, dReal x, dReal y, dReal z)
|
||||
{
|
||||
dAASSERT (b);
|
||||
b->lvel[0] = x;
|
||||
b->lvel[1] = y;
|
||||
b->lvel[2] = z;
|
||||
}
|
||||
|
||||
|
||||
void dBodySetAngularVel (dBodyID b, dReal x, dReal y, dReal z)
|
||||
{
|
||||
dAASSERT (b);
|
||||
b->avel[0] = x;
|
||||
b->avel[1] = y;
|
||||
b->avel[2] = z;
|
||||
}
|
||||
|
||||
|
||||
const dReal * dBodyGetPosition (dBodyID b)
|
||||
{
|
||||
dAASSERT (b);
|
||||
return b->pos;
|
||||
}
|
||||
|
||||
|
||||
const dReal * dBodyGetRotation (dBodyID b)
|
||||
{
|
||||
dAASSERT (b);
|
||||
return b->R;
|
||||
}
|
||||
|
||||
|
||||
const dReal * dBodyGetQuaternion (dBodyID b)
|
||||
{
|
||||
dAASSERT (b);
|
||||
return b->q;
|
||||
}
|
||||
|
||||
|
||||
const dReal * dBodyGetLinearVel (dBodyID b)
|
||||
{
|
||||
dAASSERT (b);
|
||||
return b->lvel;
|
||||
}
|
||||
|
||||
|
||||
const dReal * dBodyGetAngularVel (dBodyID b)
|
||||
{
|
||||
dAASSERT (b);
|
||||
return b->avel;
|
||||
}
|
||||
|
||||
|
||||
void dBodySetMass (dBodyID b, const dMass *mass)
|
||||
{
|
||||
dAASSERT (b && mass);
|
||||
memcpy (&b->mass,mass,sizeof(dMass));
|
||||
if (dInvertPDMatrix (b->mass.I,b->invI,3)==0) {
|
||||
dDEBUGMSG ("inertia must be positive definite");
|
||||
dRSetIdentity (b->invI);
|
||||
}
|
||||
b->invMass = dRecip(b->mass.mass);
|
||||
}
|
||||
|
||||
|
||||
void dBodyGetMass (dBodyID b, dMass *mass)
|
||||
{
|
||||
dAASSERT (b && mass);
|
||||
memcpy (mass,&b->mass,sizeof(dMass));
|
||||
}
|
||||
|
||||
|
||||
void dBodyAddForce (dBodyID b, dReal fx, dReal fy, dReal fz)
|
||||
{
|
||||
dAASSERT (b);
|
||||
b->facc[0] += fx;
|
||||
b->facc[1] += fy;
|
||||
b->facc[2] += fz;
|
||||
}
|
||||
|
||||
|
||||
void dBodyAddTorque (dBodyID b, dReal fx, dReal fy, dReal fz)
|
||||
{
|
||||
dAASSERT (b);
|
||||
b->tacc[0] += fx;
|
||||
b->tacc[1] += fy;
|
||||
b->tacc[2] += fz;
|
||||
}
|
||||
|
||||
|
||||
void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz)
|
||||
{
|
||||
dAASSERT (b);
|
||||
dVector3 t1,t2;
|
||||
t1[0] = fx;
|
||||
t1[1] = fy;
|
||||
t1[2] = fz;
|
||||
t1[3] = 0;
|
||||
dMULTIPLY0_331 (t2,b->R,t1);
|
||||
b->facc[0] += t2[0];
|
||||
b->facc[1] += t2[1];
|
||||
b->facc[2] += t2[2];
|
||||
}
|
||||
|
||||
|
||||
void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz)
|
||||
{
|
||||
dAASSERT (b);
|
||||
dVector3 t1,t2;
|
||||
t1[0] = fx;
|
||||
t1[1] = fy;
|
||||
t1[2] = fz;
|
||||
t1[3] = 0;
|
||||
dMULTIPLY0_331 (t2,b->R,t1);
|
||||
b->tacc[0] += t2[0];
|
||||
b->tacc[1] += t2[1];
|
||||
b->tacc[2] += t2[2];
|
||||
}
|
||||
|
||||
|
||||
void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
|
||||
dReal px, dReal py, dReal pz)
|
||||
{
|
||||
dAASSERT (b);
|
||||
b->facc[0] += fx;
|
||||
b->facc[1] += fy;
|
||||
b->facc[2] += fz;
|
||||
dVector3 f,q;
|
||||
f[0] = fx;
|
||||
f[1] = fy;
|
||||
f[2] = fz;
|
||||
q[0] = px - b->pos[0];
|
||||
q[1] = py - b->pos[1];
|
||||
q[2] = pz - b->pos[2];
|
||||
dCROSS (b->tacc,+=,q,f);
|
||||
}
|
||||
|
||||
|
||||
void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
|
||||
dReal px, dReal py, dReal pz)
|
||||
{
|
||||
dAASSERT (b);
|
||||
dVector3 prel,f,p;
|
||||
f[0] = fx;
|
||||
f[1] = fy;
|
||||
f[2] = fz;
|
||||
f[3] = 0;
|
||||
prel[0] = px;
|
||||
prel[1] = py;
|
||||
prel[2] = pz;
|
||||
prel[3] = 0;
|
||||
dMULTIPLY0_331 (p,b->R,prel);
|
||||
b->facc[0] += f[0];
|
||||
b->facc[1] += f[1];
|
||||
b->facc[2] += f[2];
|
||||
dCROSS (b->tacc,+=,p,f);
|
||||
}
|
||||
|
||||
|
||||
void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
|
||||
dReal px, dReal py, dReal pz)
|
||||
{
|
||||
dAASSERT (b);
|
||||
dVector3 frel,f;
|
||||
frel[0] = fx;
|
||||
frel[1] = fy;
|
||||
frel[2] = fz;
|
||||
frel[3] = 0;
|
||||
dMULTIPLY0_331 (f,b->R,frel);
|
||||
b->facc[0] += f[0];
|
||||
b->facc[1] += f[1];
|
||||
b->facc[2] += f[2];
|
||||
dVector3 q;
|
||||
q[0] = px - b->pos[0];
|
||||
q[1] = py - b->pos[1];
|
||||
q[2] = pz - b->pos[2];
|
||||
dCROSS (b->tacc,+=,q,f);
|
||||
}
|
||||
|
||||
|
||||
void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
|
||||
dReal px, dReal py, dReal pz)
|
||||
{
|
||||
dAASSERT (b);
|
||||
dVector3 frel,prel,f,p;
|
||||
frel[0] = fx;
|
||||
frel[1] = fy;
|
||||
frel[2] = fz;
|
||||
frel[3] = 0;
|
||||
prel[0] = px;
|
||||
prel[1] = py;
|
||||
prel[2] = pz;
|
||||
prel[3] = 0;
|
||||
dMULTIPLY0_331 (f,b->R,frel);
|
||||
dMULTIPLY0_331 (p,b->R,prel);
|
||||
b->facc[0] += f[0];
|
||||
b->facc[1] += f[1];
|
||||
b->facc[2] += f[2];
|
||||
dCROSS (b->tacc,+=,p,f);
|
||||
}
|
||||
|
||||
|
||||
const dReal * dBodyGetForce (dBodyID b)
|
||||
{
|
||||
dAASSERT (b);
|
||||
return b->facc;
|
||||
}
|
||||
|
||||
|
||||
const dReal * dBodyGetTorque (dBodyID b)
|
||||
{
|
||||
dAASSERT (b);
|
||||
return b->tacc;
|
||||
}
|
||||
|
||||
|
||||
void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z)
|
||||
{
|
||||
dAASSERT (b);
|
||||
b->facc[0] = x;
|
||||
b->facc[1] = y;
|
||||
b->facc[2] = z;
|
||||
}
|
||||
|
||||
|
||||
void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z)
|
||||
{
|
||||
dAASSERT (b);
|
||||
b->tacc[0] = x;
|
||||
b->tacc[1] = y;
|
||||
b->tacc[2] = z;
|
||||
}
|
||||
|
||||
|
||||
void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz,
|
||||
dVector3 result)
|
||||
{
|
||||
dAASSERT (b);
|
||||
dVector3 prel,p;
|
||||
prel[0] = px;
|
||||
prel[1] = py;
|
||||
prel[2] = pz;
|
||||
prel[3] = 0;
|
||||
dMULTIPLY0_331 (p,b->R,prel);
|
||||
result[0] = p[0] + b->pos[0];
|
||||
result[1] = p[1] + b->pos[1];
|
||||
result[2] = p[2] + b->pos[2];
|
||||
}
|
||||
|
||||
|
||||
void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz,
|
||||
dVector3 result)
|
||||
{
|
||||
dAASSERT (b);
|
||||
dVector3 prel,p;
|
||||
prel[0] = px;
|
||||
prel[1] = py;
|
||||
prel[2] = pz;
|
||||
prel[3] = 0;
|
||||
dMULTIPLY0_331 (p,b->R,prel);
|
||||
result[0] = b->lvel[0];
|
||||
result[1] = b->lvel[1];
|
||||
result[2] = b->lvel[2];
|
||||
dCROSS (result,+=,b->avel,p);
|
||||
}
|
||||
|
||||
|
||||
void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz,
|
||||
dVector3 result)
|
||||
{
|
||||
dAASSERT (b);
|
||||
dVector3 p;
|
||||
p[0] = px - b->pos[0];
|
||||
p[1] = py - b->pos[1];
|
||||
p[2] = pz - b->pos[2];
|
||||
p[3] = 0;
|
||||
result[0] = b->lvel[0];
|
||||
result[1] = b->lvel[1];
|
||||
result[2] = b->lvel[2];
|
||||
dCROSS (result,+=,b->avel,p);
|
||||
}
|
||||
|
||||
|
||||
void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz,
|
||||
dVector3 result)
|
||||
{
|
||||
dAASSERT (b);
|
||||
dVector3 prel;
|
||||
prel[0] = px - b->pos[0];
|
||||
prel[1] = py - b->pos[1];
|
||||
prel[2] = pz - b->pos[2];
|
||||
prel[3] = 0;
|
||||
dMULTIPLY1_331 (result,b->R,prel);
|
||||
}
|
||||
|
||||
|
||||
void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz,
|
||||
dVector3 result)
|
||||
{
|
||||
dAASSERT (b);
|
||||
dVector3 p;
|
||||
p[0] = px;
|
||||
p[1] = py;
|
||||
p[2] = pz;
|
||||
p[3] = 0;
|
||||
dMULTIPLY0_331 (result,b->R,p);
|
||||
}
|
||||
|
||||
|
||||
void dBodyVectorFromWorld (dBodyID b, dReal px, dReal py, dReal pz,
|
||||
dVector3 result)
|
||||
{
|
||||
dAASSERT (b);
|
||||
dVector3 p;
|
||||
p[0] = px;
|
||||
p[1] = py;
|
||||
p[2] = pz;
|
||||
p[3] = 0;
|
||||
dMULTIPLY1_331 (result,b->R,p);
|
||||
}
|
||||
|
||||
|
||||
void dBodySetFiniteRotationMode (dBodyID b, int mode)
|
||||
{
|
||||
dAASSERT (b);
|
||||
b->flags &= ~(dxBodyFlagFiniteRotation | dxBodyFlagFiniteRotationAxis);
|
||||
if (mode) {
|
||||
b->flags |= dxBodyFlagFiniteRotation;
|
||||
if (b->finite_rot_axis[0] != 0 || b->finite_rot_axis[1] != 0 ||
|
||||
b->finite_rot_axis[2] != 0) {
|
||||
b->flags |= dxBodyFlagFiniteRotationAxis;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dBodySetFiniteRotationAxis (dBodyID b, dReal x, dReal y, dReal z)
|
||||
{
|
||||
dAASSERT (b);
|
||||
b->finite_rot_axis[0] = x;
|
||||
b->finite_rot_axis[1] = y;
|
||||
b->finite_rot_axis[2] = z;
|
||||
if (x != 0 || y != 0 || z != 0) {
|
||||
dNormalize3 (b->finite_rot_axis);
|
||||
b->flags |= dxBodyFlagFiniteRotationAxis;
|
||||
}
|
||||
else {
|
||||
b->flags &= ~dxBodyFlagFiniteRotationAxis;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int dBodyGetFiniteRotationMode (dBodyID b)
|
||||
{
|
||||
dAASSERT (b);
|
||||
return ((b->flags & dxBodyFlagFiniteRotation) != 0);
|
||||
}
|
||||
|
||||
|
||||
void dBodyGetFiniteRotationAxis (dBodyID b, dVector3 result)
|
||||
{
|
||||
dAASSERT (b);
|
||||
result[0] = b->finite_rot_axis[0];
|
||||
result[1] = b->finite_rot_axis[1];
|
||||
result[2] = b->finite_rot_axis[2];
|
||||
}
|
||||
|
||||
|
||||
int dBodyGetNumJoints (dBodyID b)
|
||||
{
|
||||
dAASSERT (b);
|
||||
int count=0;
|
||||
for (dxJointNode *n=b->firstjoint; n; n=n->next, count++);
|
||||
return count;
|
||||
}
|
||||
|
||||
|
||||
dJointID dBodyGetJoint (dBodyID b, int index)
|
||||
{
|
||||
dAASSERT (b);
|
||||
int i=0;
|
||||
for (dxJointNode *n=b->firstjoint; n; n=n->next, i++) {
|
||||
if (i == index) return n->joint;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
void dBodyEnable (dBodyID b)
|
||||
{
|
||||
dAASSERT (b);
|
||||
b->flags &= ~dxBodyDisabled;
|
||||
}
|
||||
|
||||
|
||||
void dBodyDisable (dBodyID b)
|
||||
{
|
||||
dAASSERT (b);
|
||||
b->flags |= dxBodyDisabled;
|
||||
}
|
||||
|
||||
|
||||
int dBodyIsEnabled (dBodyID b)
|
||||
{
|
||||
dAASSERT (b);
|
||||
return ((b->flags & dxBodyDisabled) == 0);
|
||||
}
|
||||
|
||||
|
||||
void dBodySetGravityMode (dBodyID b, int mode)
|
||||
{
|
||||
dAASSERT (b);
|
||||
if (mode) b->flags &= ~dxBodyNoGravity;
|
||||
else b->flags |= dxBodyNoGravity;
|
||||
}
|
||||
|
||||
|
||||
int dBodyGetGravityMode (dBodyID b)
|
||||
{
|
||||
dAASSERT (b);
|
||||
return ((b->flags & dxBodyNoGravity) == 0);
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// joints
|
||||
|
||||
static void dJointInit (dxWorld *w, dxJoint *j)
|
||||
{
|
||||
dIASSERT (w && j);
|
||||
initObject (j,w);
|
||||
j->vtable = 0;
|
||||
j->flags = 0;
|
||||
j->node[0].joint = j;
|
||||
j->node[0].body = 0;
|
||||
j->node[0].next = 0;
|
||||
j->node[1].joint = j;
|
||||
j->node[1].body = 0;
|
||||
j->node[1].next = 0;
|
||||
addObjectToList (j,(dObject **) &w->firstjoint);
|
||||
w->nj++;
|
||||
}
|
||||
|
||||
|
||||
static dxJoint *createJoint (dWorldID w, dJointGroupID group,
|
||||
dxJoint::Vtable *vtable)
|
||||
{
|
||||
dIASSERT (w && vtable);
|
||||
dxJoint *j;
|
||||
if (group) {
|
||||
j = (dxJoint*) group->stack.alloc (vtable->size);
|
||||
group->num++;
|
||||
}
|
||||
else j = (dxJoint*) dAlloc (vtable->size);
|
||||
dJointInit (w,j);
|
||||
j->vtable = vtable;
|
||||
if (group) j->flags |= dJOINT_INGROUP;
|
||||
if (vtable->init) vtable->init (j);
|
||||
j->feedback = 0;
|
||||
return j;
|
||||
}
|
||||
|
||||
|
||||
dxJoint * dJointCreateBall (dWorldID w, dJointGroupID group)
|
||||
{
|
||||
dAASSERT (w);
|
||||
return createJoint (w,group,&__dball_vtable);
|
||||
}
|
||||
|
||||
|
||||
dxJoint * dJointCreateHinge (dWorldID w, dJointGroupID group)
|
||||
{
|
||||
dAASSERT (w);
|
||||
return createJoint (w,group,&__dhinge_vtable);
|
||||
}
|
||||
|
||||
|
||||
dxJoint * dJointCreateSlider (dWorldID w, dJointGroupID group)
|
||||
{
|
||||
dAASSERT (w);
|
||||
return createJoint (w,group,&__dslider_vtable);
|
||||
}
|
||||
|
||||
|
||||
dxJoint * dJointCreateContact (dWorldID w, dJointGroupID group,
|
||||
const dContact *c)
|
||||
{
|
||||
dAASSERT (w && c);
|
||||
dxJointContact *j = (dxJointContact *)
|
||||
createJoint (w,group,&__dcontact_vtable);
|
||||
j->contact = *c;
|
||||
return j;
|
||||
}
|
||||
|
||||
|
||||
dxJoint * dJointCreateHinge2 (dWorldID w, dJointGroupID group)
|
||||
{
|
||||
dAASSERT (w);
|
||||
return createJoint (w,group,&__dhinge2_vtable);
|
||||
}
|
||||
|
||||
|
||||
dxJoint * dJointCreateUniversal (dWorldID w, dJointGroupID group)
|
||||
{
|
||||
dAASSERT (w);
|
||||
return createJoint (w,group,&__duniversal_vtable);
|
||||
}
|
||||
|
||||
|
||||
dxJoint * dJointCreateFixed (dWorldID w, dJointGroupID group)
|
||||
{
|
||||
dAASSERT (w);
|
||||
return createJoint (w,group,&__dfixed_vtable);
|
||||
}
|
||||
|
||||
|
||||
dxJoint * dJointCreateNull (dWorldID w, dJointGroupID group)
|
||||
{
|
||||
dAASSERT (w);
|
||||
return createJoint (w,group,&__dnull_vtable);
|
||||
}
|
||||
|
||||
|
||||
dxJoint * dJointCreateAMotor (dWorldID w, dJointGroupID group)
|
||||
{
|
||||
dAASSERT (w);
|
||||
return createJoint (w,group,&__damotor_vtable);
|
||||
}
|
||||
|
||||
|
||||
void dJointDestroy (dxJoint *j)
|
||||
{
|
||||
dAASSERT (j);
|
||||
if (j->flags & dJOINT_INGROUP) return;
|
||||
removeJointReferencesFromAttachedBodies (j);
|
||||
removeObjectFromList (j);
|
||||
j->world->nj--;
|
||||
dFree (j,j->vtable->size);
|
||||
}
|
||||
|
||||
|
||||
dJointGroupID dJointGroupCreate (int max_size)
|
||||
{
|
||||
// not any more ... dUASSERT (max_size > 0,"max size must be > 0");
|
||||
dxJointGroup *group = new dxJointGroup;
|
||||
group->num = 0;
|
||||
return group;
|
||||
}
|
||||
|
||||
|
||||
void dJointGroupDestroy (dJointGroupID group)
|
||||
{
|
||||
dAASSERT (group);
|
||||
dJointGroupEmpty (group);
|
||||
delete group;
|
||||
}
|
||||
|
||||
|
||||
void dJointGroupEmpty (dJointGroupID group)
|
||||
{
|
||||
// the joints in this group are detached starting from the most recently
|
||||
// added (at the top of the stack). this helps ensure that the various
|
||||
// linked lists are not traversed too much, as the joints will hopefully
|
||||
// be at the start of those lists.
|
||||
// if any group joints have their world pointer set to 0, their world was
|
||||
// previously destroyed. no special handling is required for these joints.
|
||||
|
||||
dAASSERT (group);
|
||||
int i;
|
||||
dxJoint **jlist = (dxJoint**) ALLOCA (group->num * sizeof(dxJoint*));
|
||||
dxJoint *j = (dxJoint*) group->stack.rewind();
|
||||
for (i=0; i < group->num; i++) {
|
||||
jlist[i] = j;
|
||||
j = (dxJoint*) (group->stack.next (j->vtable->size));
|
||||
}
|
||||
for (i=group->num-1; i >= 0; i--) {
|
||||
if (jlist[i]->world) {
|
||||
removeJointReferencesFromAttachedBodies (jlist[i]);
|
||||
removeObjectFromList (jlist[i]);
|
||||
jlist[i]->world->nj--;
|
||||
}
|
||||
}
|
||||
group->num = 0;
|
||||
group->stack.freeAll();
|
||||
}
|
||||
|
||||
|
||||
void dJointAttach (dxJoint *joint, dxBody *body1, dxBody *body2)
|
||||
{
|
||||
// check arguments
|
||||
dUASSERT (joint,"bad joint argument");
|
||||
dUASSERT (body1 == 0 || body1 != body2,"can't have body1==body2");
|
||||
dxWorld *world = joint->world;
|
||||
dUASSERT ( (!body1 || body1->world == world) &&
|
||||
(!body2 || body2->world == world),
|
||||
"joint and bodies must be in same world");
|
||||
|
||||
// check if the joint can not be attached to just one body
|
||||
dUASSERT (!((joint->flags & dJOINT_TWOBODIES) &&
|
||||
((body1 != 0) ^ (body2 != 0))),
|
||||
"joint can not be attached to just one body");
|
||||
|
||||
// remove any existing body attachments
|
||||
if (joint->node[0].body || joint->node[1].body) {
|
||||
removeJointReferencesFromAttachedBodies (joint);
|
||||
}
|
||||
|
||||
// if a body is zero, make sure that it is body2, so 0 --> node[1].body
|
||||
if (body1==0) {
|
||||
body1 = body2;
|
||||
body2 = 0;
|
||||
joint->flags &= (~dJOINT_REVERSE);
|
||||
}
|
||||
else {
|
||||
joint->flags |= dJOINT_REVERSE;
|
||||
}
|
||||
|
||||
// attach to new bodies
|
||||
joint->node[0].body = body1;
|
||||
joint->node[1].body = body2;
|
||||
if (body1) {
|
||||
joint->node[1].next = body1->firstjoint;
|
||||
body1->firstjoint = &joint->node[1];
|
||||
}
|
||||
else joint->node[1].next = 0;
|
||||
if (body2) {
|
||||
joint->node[0].next = body2->firstjoint;
|
||||
body2->firstjoint = &joint->node[0];
|
||||
}
|
||||
else {
|
||||
joint->node[0].next = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dJointSetData (dxJoint *joint, void *data)
|
||||
{
|
||||
dAASSERT (joint);
|
||||
joint->userdata = data;
|
||||
}
|
||||
|
||||
|
||||
void *dJointGetData (dxJoint *joint)
|
||||
{
|
||||
dAASSERT (joint);
|
||||
return joint->userdata;
|
||||
}
|
||||
|
||||
|
||||
int dJointGetType (dxJoint *joint)
|
||||
{
|
||||
dAASSERT (joint);
|
||||
return joint->vtable->typenum;
|
||||
}
|
||||
|
||||
|
||||
dBodyID dJointGetBody (dxJoint *joint, int index)
|
||||
{
|
||||
dAASSERT (joint);
|
||||
if (index >= 0 && index < 2) return joint->node[index].body;
|
||||
else return 0;
|
||||
}
|
||||
|
||||
|
||||
void dJointSetFeedback (dxJoint *joint, dJointFeedback *f)
|
||||
{
|
||||
dAASSERT (joint);
|
||||
joint->feedback = f;
|
||||
}
|
||||
|
||||
|
||||
dJointFeedback *dJointGetFeedback (dxJoint *joint)
|
||||
{
|
||||
dAASSERT (joint);
|
||||
return joint->feedback;
|
||||
}
|
||||
|
||||
|
||||
int dAreConnected (dBodyID b1, dBodyID b2)
|
||||
{
|
||||
dAASSERT (b1 && b2);
|
||||
// look through b1's neighbour list for b2
|
||||
for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
|
||||
if (n->body == b2) return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// world
|
||||
|
||||
dxWorld * dWorldCreate()
|
||||
{
|
||||
dxWorld *w = new dxWorld;
|
||||
w->firstbody = 0;
|
||||
w->firstjoint = 0;
|
||||
w->nb = 0;
|
||||
w->nj = 0;
|
||||
dSetZero (w->gravity,4);
|
||||
w->global_erp = REAL(0.2);
|
||||
#if defined(dSINGLE)
|
||||
w->global_cfm = 1e-5f;
|
||||
#elif defined(dDOUBLE)
|
||||
w->global_cfm = 1e-10;
|
||||
#else
|
||||
#error dSINGLE or dDOUBLE must be defined
|
||||
#endif
|
||||
return w;
|
||||
}
|
||||
|
||||
|
||||
void dWorldDestroy (dxWorld *w)
|
||||
{
|
||||
// delete all bodies and joints
|
||||
dAASSERT (w);
|
||||
dxBody *nextb, *b = w->firstbody;
|
||||
while (b) {
|
||||
nextb = (dxBody*) b->next;
|
||||
delete b;
|
||||
b = nextb;
|
||||
}
|
||||
dxJoint *nextj, *j = w->firstjoint;
|
||||
while (j) {
|
||||
nextj = (dxJoint*)j->next;
|
||||
if (j->flags & dJOINT_INGROUP) {
|
||||
// the joint is part of a group, so "deactivate" it instead
|
||||
j->world = 0;
|
||||
j->node[0].body = 0;
|
||||
j->node[0].next = 0;
|
||||
j->node[1].body = 0;
|
||||
j->node[1].next = 0;
|
||||
dMessage (0,"warning: destroying world containing grouped joints");
|
||||
}
|
||||
else {
|
||||
dFree (j,j->vtable->size);
|
||||
}
|
||||
j = nextj;
|
||||
}
|
||||
delete w;
|
||||
}
|
||||
|
||||
|
||||
void dWorldSetGravity (dWorldID w, dReal x, dReal y, dReal z)
|
||||
{
|
||||
dAASSERT (w);
|
||||
w->gravity[0] = x;
|
||||
w->gravity[1] = y;
|
||||
w->gravity[2] = z;
|
||||
}
|
||||
|
||||
|
||||
void dWorldGetGravity (dWorldID w, dVector3 g)
|
||||
{
|
||||
dAASSERT (w);
|
||||
g[0] = w->gravity[0];
|
||||
g[1] = w->gravity[1];
|
||||
g[2] = w->gravity[2];
|
||||
}
|
||||
|
||||
|
||||
void dWorldSetERP (dWorldID w, dReal erp)
|
||||
{
|
||||
dAASSERT (w);
|
||||
w->global_erp = erp;
|
||||
}
|
||||
|
||||
|
||||
dReal dWorldGetERP (dWorldID w)
|
||||
{
|
||||
dAASSERT (w);
|
||||
return w->global_erp;
|
||||
}
|
||||
|
||||
|
||||
void dWorldSetCFM (dWorldID w, dReal cfm)
|
||||
{
|
||||
dAASSERT (w);
|
||||
w->global_cfm = cfm;
|
||||
}
|
||||
|
||||
|
||||
dReal dWorldGetCFM (dWorldID w)
|
||||
{
|
||||
dAASSERT (w);
|
||||
return w->global_cfm;
|
||||
}
|
||||
|
||||
|
||||
void dWorldStep (dWorldID w, dReal stepsize)
|
||||
{
|
||||
dUASSERT (w,"bad world argument");
|
||||
dUASSERT (stepsize > 0,"stepsize must be > 0");
|
||||
processIslands (w,stepsize);
|
||||
}
|
||||
|
||||
|
||||
void dWorldImpulseToForce (dWorldID w, dReal stepsize,
|
||||
dReal ix, dReal iy, dReal iz,
|
||||
dVector3 force)
|
||||
{
|
||||
dAASSERT (w);
|
||||
stepsize = dRecip(stepsize);
|
||||
force[0] = stepsize * ix;
|
||||
force[1] = stepsize * iy;
|
||||
force[2] = stepsize * iz;
|
||||
// @@@ force[3] = 0;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// testing
|
||||
|
||||
#define NUM 100
|
||||
|
||||
#define DO(x)
|
||||
|
||||
|
||||
extern "C" void dTestDataStructures()
|
||||
{
|
||||
int i;
|
||||
DO(printf ("testDynamicsStuff()\n"));
|
||||
|
||||
dBodyID body [NUM];
|
||||
int nb = 0;
|
||||
dJointID joint [NUM];
|
||||
int nj = 0;
|
||||
|
||||
for (i=0; i<NUM; i++) body[i] = 0;
|
||||
for (i=0; i<NUM; i++) joint[i] = 0;
|
||||
|
||||
DO(printf ("creating world\n"));
|
||||
dWorldID w = dWorldCreate();
|
||||
checkWorld (w);
|
||||
|
||||
for (;;) {
|
||||
if (nb < NUM && dRandReal() > 0.5) {
|
||||
DO(printf ("creating body\n"));
|
||||
body[nb] = dBodyCreate (w);
|
||||
DO(printf ("\t--> %p\n",body[nb]));
|
||||
nb++;
|
||||
checkWorld (w);
|
||||
DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
|
||||
}
|
||||
if (nj < NUM && nb > 2 && dRandReal() > 0.5) {
|
||||
dBodyID b1 = body [dRand() % nb];
|
||||
dBodyID b2 = body [dRand() % nb];
|
||||
if (b1 != b2) {
|
||||
DO(printf ("creating joint, attaching to %p,%p\n",b1,b2));
|
||||
joint[nj] = dJointCreateBall (w,0);
|
||||
DO(printf ("\t-->%p\n",joint[nj]));
|
||||
checkWorld (w);
|
||||
dJointAttach (joint[nj],b1,b2);
|
||||
nj++;
|
||||
checkWorld (w);
|
||||
DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
|
||||
}
|
||||
}
|
||||
if (nj > 0 && nb > 2 && dRandReal() > 0.5) {
|
||||
dBodyID b1 = body [dRand() % nb];
|
||||
dBodyID b2 = body [dRand() % nb];
|
||||
if (b1 != b2) {
|
||||
int k = dRand() % nj;
|
||||
DO(printf ("reattaching joint %p\n",joint[k]));
|
||||
dJointAttach (joint[k],b1,b2);
|
||||
checkWorld (w);
|
||||
DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
|
||||
}
|
||||
}
|
||||
if (nb > 0 && dRandReal() > 0.5) {
|
||||
int k = dRand() % nb;
|
||||
DO(printf ("destroying body %p\n",body[k]));
|
||||
dBodyDestroy (body[k]);
|
||||
checkWorld (w);
|
||||
for (; k < (NUM-1); k++) body[k] = body[k+1];
|
||||
nb--;
|
||||
DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
|
||||
}
|
||||
if (nj > 0 && dRandReal() > 0.5) {
|
||||
int k = dRand() % nj;
|
||||
DO(printf ("destroying joint %p\n",joint[k]));
|
||||
dJointDestroy (joint[k]);
|
||||
checkWorld (w);
|
||||
for (; k < (NUM-1); k++) joint[k] = joint[k+1];
|
||||
nj--;
|
||||
DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
printf ("creating world\n");
|
||||
dWorldID w = dWorldCreate();
|
||||
checkWorld (w);
|
||||
printf ("creating body\n");
|
||||
dBodyID b1 = dBodyCreate (w);
|
||||
checkWorld (w);
|
||||
printf ("creating body\n");
|
||||
dBodyID b2 = dBodyCreate (w);
|
||||
checkWorld (w);
|
||||
printf ("creating joint\n");
|
||||
dJointID j = dJointCreateBall (w);
|
||||
checkWorld (w);
|
||||
printf ("attaching joint\n");
|
||||
dJointAttach (j,b1,b2);
|
||||
checkWorld (w);
|
||||
printf ("destroying joint\n");
|
||||
dJointDestroy (j);
|
||||
checkWorld (w);
|
||||
printf ("destroying body\n");
|
||||
dBodyDestroy (b1);
|
||||
checkWorld (w);
|
||||
printf ("destroying body\n");
|
||||
dBodyDestroy (b2);
|
||||
checkWorld (w);
|
||||
printf ("destroying world\n");
|
||||
dWorldDestroy (w);
|
||||
*/
|
||||
}
|
||||
173
extern/ode/dist/ode/src/odemath.cpp
vendored
173
extern/ode/dist/ode/src/odemath.cpp
vendored
@@ -1,173 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#define SHARED_CONFIG_H_INCLUDED_FROM_DEFINING_FILE 1
|
||||
#include <ode/common.h>
|
||||
#include <ode/odemath.h>
|
||||
|
||||
|
||||
// get some math functions under windows
|
||||
#ifdef WIN32
|
||||
#include <float.h>
|
||||
#ifndef CYGWIN // added by andy for cygwin
|
||||
#define copysign(a,b) ((dReal)_copysign(a,b))
|
||||
#endif // added by andy for cygwin
|
||||
#endif
|
||||
|
||||
|
||||
// infinity declaration
|
||||
|
||||
#ifdef DINFINITY_DECL
|
||||
DINFINITY_DECL
|
||||
#endif
|
||||
|
||||
|
||||
// this may be called for vectors `a' with extremely small magnitude, for
|
||||
// example the result of a cross product on two nearly perpendicular vectors.
|
||||
// we must be robust to these small vectors. to prevent numerical error,
|
||||
// first find the component a[i] with the largest magnitude and then scale
|
||||
// all the components by 1/a[i]. then we can compute the length of `a' and
|
||||
// scale the components by 1/l. this has been verified to work with vectors
|
||||
// containing the smallest representable numbers.
|
||||
|
||||
void dNormalize3 (dVector3 a)
|
||||
{
|
||||
dReal a0,a1,a2,aa0,aa1,aa2,l;
|
||||
dAASSERT (a);
|
||||
a0 = a[0];
|
||||
a1 = a[1];
|
||||
a2 = a[2];
|
||||
aa0 = dFabs(a0);
|
||||
aa1 = dFabs(a1);
|
||||
aa2 = dFabs(a2);
|
||||
if (aa1 > aa0) {
|
||||
if (aa2 > aa1) {
|
||||
goto aa2_largest;
|
||||
}
|
||||
else { // aa1 is largest
|
||||
a0 /= aa1;
|
||||
a2 /= aa1;
|
||||
l = dRecipSqrt (a0*a0 + a2*a2 + 1);
|
||||
a[0] = a0*l;
|
||||
a[1] = copysign(l,a1);
|
||||
a[2] = a2*l;
|
||||
}
|
||||
}
|
||||
else {
|
||||
if (aa2 > aa0) {
|
||||
aa2_largest: // aa2 is largest
|
||||
a0 /= aa2;
|
||||
a1 /= aa2;
|
||||
l = dRecipSqrt (a0*a0 + a1*a1 + 1);
|
||||
a[0] = a0*l;
|
||||
a[1] = a1*l;
|
||||
a[2] = copysign(l,a2);
|
||||
}
|
||||
else { // aa0 is largest
|
||||
if (aa0 <= 0) {
|
||||
dDEBUGMSG ("vector has zero size");
|
||||
a[0] = 1; // if all a's are zero, this is where we'll end up.
|
||||
a[1] = 0; // return a default unit length vector.
|
||||
a[2] = 0;
|
||||
return;
|
||||
}
|
||||
a1 /= aa0;
|
||||
a2 /= aa0;
|
||||
l = dRecipSqrt (a1*a1 + a2*a2 + 1);
|
||||
a[0] = copysign(l,a0);
|
||||
a[1] = a1*l;
|
||||
a[2] = a2*l;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* OLD VERSION */
|
||||
/*
|
||||
void dNormalize3 (dVector3 a)
|
||||
{
|
||||
dASSERT (a);
|
||||
dReal l = dDOT(a,a);
|
||||
if (l > 0) {
|
||||
l = dRecipSqrt(l);
|
||||
a[0] *= l;
|
||||
a[1] *= l;
|
||||
a[2] *= l;
|
||||
}
|
||||
else {
|
||||
a[0] = 1;
|
||||
a[1] = 0;
|
||||
a[2] = 0;
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
void dNormalize4 (dVector4 a)
|
||||
{
|
||||
dAASSERT (a);
|
||||
dReal l = dDOT(a,a)+a[3]*a[3];
|
||||
if (l > 0) {
|
||||
l = dRecipSqrt(l);
|
||||
a[0] *= l;
|
||||
a[1] *= l;
|
||||
a[2] *= l;
|
||||
a[3] *= l;
|
||||
}
|
||||
else {
|
||||
dDEBUGMSG ("vector has zero size");
|
||||
a[0] = 1;
|
||||
a[1] = 0;
|
||||
a[2] = 0;
|
||||
a[3] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q)
|
||||
{
|
||||
dAASSERT (n && p && q);
|
||||
if (dFabs(n[2]) > M_SQRT1_2) {
|
||||
// choose p in y-z plane
|
||||
dReal a = n[1]*n[1] + n[2]*n[2];
|
||||
dReal k = dRecipSqrt (a);
|
||||
p[0] = 0;
|
||||
p[1] = -n[2]*k;
|
||||
p[2] = n[1]*k;
|
||||
// set q = n x p
|
||||
q[0] = a*k;
|
||||
q[1] = -n[0]*p[2];
|
||||
q[2] = n[0]*p[1];
|
||||
}
|
||||
else {
|
||||
// choose p in x-y plane
|
||||
dReal a = n[0]*n[0] + n[1]*n[1];
|
||||
dReal k = dRecipSqrt (a);
|
||||
p[0] = -n[1]*k;
|
||||
p[1] = n[0]*k;
|
||||
p[2] = 0;
|
||||
// set q = n x p
|
||||
q[0] = -n[2]*p[1];
|
||||
q[1] = n[2]*p[0];
|
||||
q[2] = a*k;
|
||||
}
|
||||
}
|
||||
283
extern/ode/dist/ode/src/rotation.cpp
vendored
283
extern/ode/dist/ode/src/rotation.cpp
vendored
@@ -1,283 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
/*
|
||||
|
||||
quaternions have the format: (s,vx,vy,vz) where (vx,vy,vz) is the
|
||||
"rotation axis" and s is the "rotation angle".
|
||||
|
||||
*/
|
||||
|
||||
#include <ode/rotation.h>
|
||||
|
||||
|
||||
#define _R(i,j) R[(i)*4+(j)]
|
||||
|
||||
#define SET_3x3_IDENTITY \
|
||||
_R(0,0) = REAL(1.0); \
|
||||
_R(0,1) = REAL(0.0); \
|
||||
_R(0,2) = REAL(0.0); \
|
||||
_R(0,3) = REAL(0.0); \
|
||||
_R(1,0) = REAL(0.0); \
|
||||
_R(1,1) = REAL(1.0); \
|
||||
_R(1,2) = REAL(0.0); \
|
||||
_R(1,3) = REAL(0.0); \
|
||||
_R(2,0) = REAL(0.0); \
|
||||
_R(2,1) = REAL(0.0); \
|
||||
_R(2,2) = REAL(1.0); \
|
||||
_R(2,3) = REAL(0.0);
|
||||
|
||||
|
||||
void dRSetIdentity (dMatrix3 R)
|
||||
{
|
||||
dAASSERT (R);
|
||||
SET_3x3_IDENTITY;
|
||||
}
|
||||
|
||||
|
||||
void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az,
|
||||
dReal angle)
|
||||
{
|
||||
dAASSERT (R);
|
||||
dQuaternion q;
|
||||
dQFromAxisAndAngle (q,ax,ay,az,angle);
|
||||
dQtoR (q,R);
|
||||
}
|
||||
|
||||
|
||||
void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi)
|
||||
{
|
||||
dReal sphi,cphi,stheta,ctheta,spsi,cpsi;
|
||||
dAASSERT (R);
|
||||
sphi = dSin(phi);
|
||||
cphi = dCos(phi);
|
||||
stheta = dSin(theta);
|
||||
ctheta = dCos(theta);
|
||||
spsi = dSin(psi);
|
||||
cpsi = dCos(psi);
|
||||
_R(0,0) = cpsi*ctheta;
|
||||
_R(0,1) = spsi*ctheta;
|
||||
_R(0,2) =-stheta;
|
||||
_R(1,0) = cpsi*stheta*sphi - spsi*cphi;
|
||||
_R(1,1) = spsi*stheta*sphi + cpsi*cphi;
|
||||
_R(1,2) = ctheta*sphi;
|
||||
_R(2,0) = cpsi*stheta*cphi + spsi*sphi;
|
||||
_R(2,1) = spsi*stheta*cphi - cpsi*sphi;
|
||||
_R(2,2) = ctheta*cphi;
|
||||
}
|
||||
|
||||
|
||||
void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az,
|
||||
dReal bx, dReal by, dReal bz)
|
||||
{
|
||||
dReal l,k;
|
||||
dAASSERT (R);
|
||||
l = dSqrt (ax*ax + ay*ay + az*az);
|
||||
if (l <= REAL(0.0)) {
|
||||
dDEBUGMSG ("zero length vector");
|
||||
return;
|
||||
}
|
||||
l = dRecip(l);
|
||||
ax *= l;
|
||||
ay *= l;
|
||||
az *= l;
|
||||
k = ax*bx + ay*by + az*bz;
|
||||
bx -= k*ax;
|
||||
by -= k*ay;
|
||||
bz -= k*az;
|
||||
l = dSqrt (bx*bx + by*by + bz*bz);
|
||||
if (l <= REAL(0.0)) {
|
||||
dDEBUGMSG ("zero length vector");
|
||||
return;
|
||||
}
|
||||
l = dRecip(l);
|
||||
bx *= l;
|
||||
by *= l;
|
||||
bz *= l;
|
||||
_R(0,0) = ax;
|
||||
_R(1,0) = ay;
|
||||
_R(2,0) = az;
|
||||
_R(0,1) = bx;
|
||||
_R(1,1) = by;
|
||||
_R(2,1) = bz;
|
||||
_R(0,2) = - by*az + ay*bz;
|
||||
_R(1,2) = - bz*ax + az*bx;
|
||||
_R(2,2) = - bx*ay + ax*by;
|
||||
}
|
||||
|
||||
|
||||
void dQSetIdentity (dQuaternion q)
|
||||
{
|
||||
dAASSERT (q);
|
||||
q[0] = 1;
|
||||
q[1] = 0;
|
||||
q[2] = 0;
|
||||
q[3] = 0;
|
||||
}
|
||||
|
||||
|
||||
void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
|
||||
dReal angle)
|
||||
{
|
||||
dAASSERT (q);
|
||||
dReal l = ax*ax + ay*ay + az*az;
|
||||
if (l > REAL(0.0)) {
|
||||
angle *= REAL(0.5);
|
||||
q[0] = dCos (angle);
|
||||
l = dSin(angle) * dRecipSqrt(l);
|
||||
q[1] = ax*l;
|
||||
q[2] = ay*l;
|
||||
q[3] = az*l;
|
||||
}
|
||||
else {
|
||||
q[0] = 1;
|
||||
q[1] = 0;
|
||||
q[2] = 0;
|
||||
q[3] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
|
||||
{
|
||||
dAASSERT (qa && qb && qc);
|
||||
qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3];
|
||||
qa[1] = qb[0]*qc[1] + qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2];
|
||||
qa[2] = qb[0]*qc[2] + qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3];
|
||||
qa[3] = qb[0]*qc[3] + qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1];
|
||||
}
|
||||
|
||||
|
||||
void dQMultiply1 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
|
||||
{
|
||||
dAASSERT (qa && qb && qc);
|
||||
qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3];
|
||||
qa[1] = qb[0]*qc[1] - qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2];
|
||||
qa[2] = qb[0]*qc[2] - qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3];
|
||||
qa[3] = qb[0]*qc[3] - qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1];
|
||||
}
|
||||
|
||||
|
||||
void dQMultiply2 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
|
||||
{
|
||||
dAASSERT (qa && qb && qc);
|
||||
qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3];
|
||||
qa[1] = -qb[0]*qc[1] + qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2];
|
||||
qa[2] = -qb[0]*qc[2] + qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3];
|
||||
qa[3] = -qb[0]*qc[3] + qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1];
|
||||
}
|
||||
|
||||
|
||||
void dQMultiply3 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
|
||||
{
|
||||
dAASSERT (qa && qb && qc);
|
||||
qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3];
|
||||
qa[1] = -qb[0]*qc[1] - qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2];
|
||||
qa[2] = -qb[0]*qc[2] - qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3];
|
||||
qa[3] = -qb[0]*qc[3] - qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1];
|
||||
}
|
||||
|
||||
|
||||
// QtoR(), RtoQ() and WtoDQ() are derived from equations in "An Introduction
|
||||
// to Physically Based Modeling: Rigid Body Simulation - 1: Unconstrained
|
||||
// Rigid Body Dynamics" by David Baraff, Robotics Institute, Carnegie Mellon
|
||||
// University, 1997.
|
||||
|
||||
void dQtoR (const dQuaternion q, dMatrix3 R)
|
||||
{
|
||||
dAASSERT (q && R);
|
||||
// q = (s,vx,vy,vz)
|
||||
dReal qq1 = 2*q[1]*q[1];
|
||||
dReal qq2 = 2*q[2]*q[2];
|
||||
dReal qq3 = 2*q[3]*q[3];
|
||||
_R(0,0) = 1 - qq2 - qq3;
|
||||
_R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
|
||||
_R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
|
||||
_R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]);
|
||||
_R(1,1) = 1 - qq1 - qq3;
|
||||
_R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]);
|
||||
_R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]);
|
||||
_R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]);
|
||||
_R(2,2) = 1 - qq1 - qq2;
|
||||
}
|
||||
|
||||
|
||||
void dRtoQ (const dMatrix3 R, dQuaternion q)
|
||||
{
|
||||
dAASSERT (q && R);
|
||||
dReal tr,s;
|
||||
tr = _R(0,0) + _R(1,1) + _R(2,2);
|
||||
if (tr >= 0) {
|
||||
s = dSqrt (tr + 1);
|
||||
q[0] = REAL(0.5) * s;
|
||||
s = REAL(0.5) * dRecip(s);
|
||||
q[1] = (_R(2,1) - _R(1,2)) * s;
|
||||
q[2] = (_R(0,2) - _R(2,0)) * s;
|
||||
q[3] = (_R(1,0) - _R(0,1)) * s;
|
||||
}
|
||||
else {
|
||||
// find the largest diagonal element and jump to the appropriate case
|
||||
if (_R(1,1) > _R(0,0)) {
|
||||
if (_R(2,2) > _R(1,1)) goto case_2;
|
||||
goto case_1;
|
||||
}
|
||||
if (_R(2,2) > _R(0,0)) goto case_2;
|
||||
goto case_0;
|
||||
|
||||
case_0:
|
||||
s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + 1);
|
||||
q[1] = REAL(0.5) * s;
|
||||
s = REAL(0.5) * dRecip(s);
|
||||
q[2] = (_R(0,1) + _R(1,0)) * s;
|
||||
q[3] = (_R(2,0) + _R(0,2)) * s;
|
||||
q[0] = (_R(2,1) - _R(1,2)) * s;
|
||||
return;
|
||||
|
||||
case_1:
|
||||
s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + 1);
|
||||
q[2] = REAL(0.5) * s;
|
||||
s = REAL(0.5) * dRecip(s);
|
||||
q[3] = (_R(1,2) + _R(2,1)) * s;
|
||||
q[1] = (_R(0,1) + _R(1,0)) * s;
|
||||
q[0] = (_R(0,2) - _R(2,0)) * s;
|
||||
return;
|
||||
|
||||
case_2:
|
||||
s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + 1);
|
||||
q[3] = REAL(0.5) * s;
|
||||
s = REAL(0.5) * dRecip(s);
|
||||
q[1] = (_R(2,0) + _R(0,2)) * s;
|
||||
q[2] = (_R(1,2) + _R(2,1)) * s;
|
||||
q[0] = (_R(1,0) - _R(0,1)) * s;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dWtoDQ (const dVector3 w, const dQuaternion q, dVector4 dq)
|
||||
{
|
||||
dAASSERT (w && q && dq);
|
||||
dq[0] = REAL(0.5)*(- w[0]*q[1] - w[1]*q[2] - w[2]*q[3]);
|
||||
dq[1] = REAL(0.5)*( w[0]*q[0] + w[1]*q[3] - w[2]*q[2]);
|
||||
dq[2] = REAL(0.5)*(- w[0]*q[3] + w[1]*q[0] + w[2]*q[1]);
|
||||
dq[3] = REAL(0.5)*( w[0]*q[2] - w[1]*q[1] + w[2]*q[0]);
|
||||
}
|
||||
270
extern/ode/dist/ode/src/scrapbook.cpp
vendored
270
extern/ode/dist/ode/src/scrapbook.cpp
vendored
@@ -1,270 +0,0 @@
|
||||
|
||||
/*
|
||||
|
||||
this is code that was once useful but has now been obseleted.
|
||||
|
||||
this file should not be compiled as part of ODE!
|
||||
|
||||
*/
|
||||
|
||||
//***************************************************************************
|
||||
// intersect a line segment with a plane
|
||||
|
||||
extern "C" int dClipLineToBox (const dVector3 p1, const dVector3 p2,
|
||||
const dVector3 p, const dMatrix3 R,
|
||||
const dVector3 side)
|
||||
{
|
||||
// compute the start and end of the line (p1 and p2) relative to the box.
|
||||
// we will do all subsequent computations in this box-relative coordinate
|
||||
// system. we have to do a translation and rotation for each point.
|
||||
dVector3 tmp,s,e;
|
||||
tmp[0] = p1[0] - p[0];
|
||||
tmp[1] = p1[1] - p[1];
|
||||
tmp[2] = p1[2] - p[2];
|
||||
dMULTIPLY1_331 (s,R,tmp);
|
||||
tmp[0] = p2[0] - p[0];
|
||||
tmp[1] = p2[1] - p[1];
|
||||
tmp[2] = p2[2] - p[2];
|
||||
dMULTIPLY1_331 (e,R,tmp);
|
||||
|
||||
// compute the vector 'v' from the start point to the end point
|
||||
dVector3 v;
|
||||
v[0] = e[0] - s[0];
|
||||
v[1] = e[1] - s[1];
|
||||
v[2] = e[2] - s[2];
|
||||
|
||||
// a point on the line is defined by the parameter 't'. t=0 corresponds
|
||||
// to the start of the line, t=1 corresponds to the end of the line.
|
||||
// we will clip the line to the box by finding the range of t where a
|
||||
// point on the line is inside the box. the currently known bounds for
|
||||
// t and tlo..thi.
|
||||
dReal tlo=0,thi=1;
|
||||
|
||||
// clip in the X/Y/Z direction
|
||||
for (int i=0; i<3; i++) {
|
||||
// first adjust s,e for the current t range. this is redundant for the
|
||||
// first iteration, but never mind.
|
||||
e[i] = s[i] + thi*v[i];
|
||||
s[i] = s[i] + tlo*v[i];
|
||||
// compute where t intersects the positive and negative sides.
|
||||
dReal tp = ( side[i] - s[i])/v[i]; // @@@ handle case where denom=0
|
||||
dReal tm = (-side[i] - s[i])/v[i];
|
||||
// handle 9 intersection cases
|
||||
if (s[i] <= -side[i]) {
|
||||
tlo = tm;
|
||||
if (e[i] <= -side[i]) return 0;
|
||||
else if (e[i] >= side[i]) thi = tp;
|
||||
}
|
||||
else if (s[i] <= side[i]) {
|
||||
if (e[i] <= -side[i]) thi = tm;
|
||||
else if (e[i] >= side[i]) thi = tp;
|
||||
}
|
||||
else {
|
||||
tlo = tp;
|
||||
if (e[i] <= -side[i]) thi = tm;
|
||||
else if (e[i] >= side[i]) return 0;
|
||||
}
|
||||
}
|
||||
|
||||
//... @@@ AT HERE @@@
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
//***************************************************************************
|
||||
// a nice try at C-B collision. unfortunately it doesn't work. the logic
|
||||
// for testing for line-box intersection is correct, but unfortunately the
|
||||
// closest-point distance estimates are often too large. as a result contact
|
||||
// points are placed incorrectly.
|
||||
|
||||
|
||||
int dCollideCB (const dxGeom *o1, const dxGeom *o2, int flags,
|
||||
dContactGeom *contact, int skip)
|
||||
{
|
||||
int i;
|
||||
|
||||
dIASSERT (skip >= (int)sizeof(dContactGeom));
|
||||
dIASSERT (o1->_class->num == dCCylinderClass);
|
||||
dIASSERT (o2->_class->num == dBoxClass);
|
||||
contact->g1 = const_cast<dxGeom*> (o1);
|
||||
contact->g2 = const_cast<dxGeom*> (o2);
|
||||
dxCCylinder *cyl = (dxCCylinder*) CLASSDATA(o1);
|
||||
dxBox *box = (dxBox*) CLASSDATA(o2);
|
||||
|
||||
// get p1,p2 = cylinder axis endpoints, get radius
|
||||
dVector3 p1,p2;
|
||||
dReal clen = cyl->lz * REAL(0.5);
|
||||
p1[0] = o1->pos[0] + clen * o1->R[2];
|
||||
p1[1] = o1->pos[1] + clen * o1->R[6];
|
||||
p1[2] = o1->pos[2] + clen * o1->R[10];
|
||||
p2[0] = o1->pos[0] - clen * o1->R[2];
|
||||
p2[1] = o1->pos[1] - clen * o1->R[6];
|
||||
p2[2] = o1->pos[2] - clen * o1->R[10];
|
||||
dReal radius = cyl->radius;
|
||||
|
||||
// copy out box center, rotation matrix, and side array
|
||||
dReal *c = o2->pos;
|
||||
dReal *R = o2->R;
|
||||
dReal *side = box->side;
|
||||
|
||||
// compute the start and end of the line (p1 and p2) relative to the box.
|
||||
// we will do all subsequent computations in this box-relative coordinate
|
||||
// system. we have to do a translation and rotation for each point.
|
||||
dVector3 tmp3,s,e;
|
||||
tmp3[0] = p1[0] - c[0];
|
||||
tmp3[1] = p1[1] - c[1];
|
||||
tmp3[2] = p1[2] - c[2];
|
||||
dMULTIPLY1_331 (s,R,tmp3);
|
||||
tmp3[0] = p2[0] - c[0];
|
||||
tmp3[1] = p2[1] - c[1];
|
||||
tmp3[2] = p2[2] - c[2];
|
||||
dMULTIPLY1_331 (e,R,tmp3);
|
||||
|
||||
// compute the vector 'v' from the start point to the end point
|
||||
dVector3 v;
|
||||
v[0] = e[0] - s[0];
|
||||
v[1] = e[1] - s[1];
|
||||
v[2] = e[2] - s[2];
|
||||
|
||||
// compute the half-sides of the box
|
||||
dReal S0 = side[0] * REAL(0.5);
|
||||
dReal S1 = side[1] * REAL(0.5);
|
||||
dReal S2 = side[2] * REAL(0.5);
|
||||
|
||||
// compute the size of the bounding box around the line segment
|
||||
dReal B0 = dFabs (v[0]);
|
||||
dReal B1 = dFabs (v[1]);
|
||||
dReal B2 = dFabs (v[2]);
|
||||
|
||||
// for all 6 separation axes, measure the penetration depth. if any depth is
|
||||
// less than 0 then the objects don't penetrate at all so we can just
|
||||
// return 0. find the axis with the smallest depth, and record its normal.
|
||||
|
||||
// note: normalR is set to point to a column of R if that is the smallest
|
||||
// depth normal so far. otherwise normalR is 0 and normalC is set to a
|
||||
// vector relative to the box. invert_normal is 1 if the sign of the normal
|
||||
// should be flipped.
|
||||
|
||||
dReal depth,trial_depth,tmp,length;
|
||||
const dReal *normalR=0;
|
||||
dVector3 normalC;
|
||||
int invert_normal = 0;
|
||||
int code = 0; // 0=no contact, 1-3=face contact, 4-6=edge contact
|
||||
|
||||
depth = dInfinity;
|
||||
|
||||
// look at face-normal axes
|
||||
|
||||
#undef TEST
|
||||
#define TEST(center,depth_expr,norm,contact_code) \
|
||||
tmp = (center); \
|
||||
trial_depth = radius + REAL(0.5) * ((depth_expr) - dFabs(tmp)); \
|
||||
if (trial_depth < 0) return 0; \
|
||||
if (trial_depth < depth) { \
|
||||
depth = trial_depth; \
|
||||
normalR = (norm); \
|
||||
invert_normal = (tmp < 0); \
|
||||
code = contact_code; \
|
||||
}
|
||||
|
||||
TEST (s[0]+e[0], side[0] + B0, R+0, 1);
|
||||
TEST (s[1]+e[1], side[1] + B1, R+1, 2);
|
||||
TEST (s[2]+e[2], side[2] + B2, R+2, 3);
|
||||
|
||||
// look at v x box-edge axes
|
||||
|
||||
#undef TEST
|
||||
#define TEST(box_radius,line_offset,nx,ny,nz,contact_code) \
|
||||
tmp = (line_offset); \
|
||||
trial_depth = (box_radius) - dFabs(tmp); \
|
||||
length = dSqrt ((nx)*(nx) + (ny)*(ny) + (nz)*(nz)); \
|
||||
if (length > 0) { \
|
||||
length = dRecip(length); \
|
||||
trial_depth = trial_depth * length + radius; \
|
||||
if (trial_depth < 0) return 0; \
|
||||
if (trial_depth < depth) { \
|
||||
depth = trial_depth; \
|
||||
normalR = 0; \
|
||||
normalC[0] = (nx)*length; \
|
||||
normalC[1] = (ny)*length; \
|
||||
normalC[2] = (nz)*length; \
|
||||
invert_normal = (tmp < 0); \
|
||||
code = contact_code; \
|
||||
} \
|
||||
}
|
||||
|
||||
TEST (B2*S1+B1*S2,v[1]*s[2]-v[2]*s[1], 0,-v[2],v[1], 4);
|
||||
TEST (B2*S0+B0*S2,v[2]*s[0]-v[0]*s[2], v[2],0,-v[0], 5);
|
||||
TEST (B1*S0+B0*S1,v[0]*s[1]-v[1]*s[0], -v[1],v[0],0, 6);
|
||||
|
||||
#undef TEST
|
||||
|
||||
// if we get to this point, the box and ccylinder interpenetrate.
|
||||
// compute the normal in global coordinates.
|
||||
dReal *normal = contact[0].normal;
|
||||
if (normalR) {
|
||||
normal[0] = normalR[0];
|
||||
normal[1] = normalR[4];
|
||||
normal[2] = normalR[8];
|
||||
}
|
||||
else {
|
||||
dMULTIPLY0_331 (normal,R,normalC);
|
||||
}
|
||||
if (invert_normal) {
|
||||
normal[0] = -normal[0];
|
||||
normal[1] = -normal[1];
|
||||
normal[2] = -normal[2];
|
||||
}
|
||||
|
||||
// set the depth
|
||||
contact[0].depth = depth;
|
||||
|
||||
if (code == 0) {
|
||||
return 0; // should never get here
|
||||
}
|
||||
else if (code >= 4) {
|
||||
// handle edge contacts
|
||||
// find an endpoint q1 on the intersecting edge of the box
|
||||
dVector3 q1;
|
||||
dReal sign[3];
|
||||
for (i=0; i<3; i++) q1[i] = c[i];
|
||||
sign[0] = (dDOT14(normal,R+0) > 0) ? REAL(1.0) : REAL(-1.0);
|
||||
for (i=0; i<3; i++) q1[i] += sign[0] * S0 * R[i*4];
|
||||
sign[1] = (dDOT14(normal,R+1) > 0) ? REAL(1.0) : REAL(-1.0);
|
||||
for (i=0; i<3; i++) q1[i] += sign[1] * S1 * R[i*4+1];
|
||||
sign[2] = (dDOT14(normal,R+2) > 0) ? REAL(1.0) : REAL(-1.0);
|
||||
for (i=0; i<3; i++) q1[i] += sign[2] * S2 * R[i*4+2];
|
||||
|
||||
// find the other endpoint q2 of the intersecting edge
|
||||
dVector3 q2;
|
||||
for (i=0; i<3; i++)
|
||||
q2[i] = q1[i] - R[code-4 + i*4] * (sign[code-4] * side[code-4]);
|
||||
|
||||
// determine the closest point between the box edge and the line segment
|
||||
dVector3 cp1,cp2;
|
||||
dClosestLineSegmentPoints (q1,q2, p1,p2, cp1,cp2);
|
||||
for (i=0; i<3; i++) contact[0].pos[i] = cp1[i] - REAL(0.5)*normal[i]*depth;
|
||||
return 1;
|
||||
}
|
||||
else {
|
||||
// handle face contacts.
|
||||
// @@@ temporary: make deepest vertex on the line the contact point.
|
||||
// @@@ this kind of works, but we sometimes need two contact points for
|
||||
// @@@ stability.
|
||||
|
||||
// compute 'v' in global coordinates
|
||||
dVector3 gv;
|
||||
for (i=0; i<3; i++) gv[i] = p2[i] - p1[i];
|
||||
|
||||
if (dDOT (normal,gv) > 0) {
|
||||
for (i=0; i<3; i++)
|
||||
contact[0].pos[i] = p1[i] + (depth*REAL(0.5)-radius)*normal[i];
|
||||
}
|
||||
else {
|
||||
for (i=0; i<3; i++)
|
||||
contact[0].pos[i] = p2[i] + (depth*REAL(0.5)-radius)*normal[i];
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
621
extern/ode/dist/ode/src/space.cpp
vendored
621
extern/ode/dist/ode/src/space.cpp
vendored
@@ -1,621 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
/*
|
||||
|
||||
simple space
|
||||
------------
|
||||
|
||||
reports all n^2 object intersections
|
||||
|
||||
|
||||
multi-resolution hash table
|
||||
---------------------------
|
||||
|
||||
the current implementation rebuilds a new hash table each time collide()
|
||||
is called. we don't keep any state between calls. this is wasteful if there
|
||||
are unmoving objects in the space.
|
||||
|
||||
|
||||
TODO
|
||||
----
|
||||
|
||||
less memory wasting may to prevent multiple collision callbacks for the
|
||||
same pair?
|
||||
|
||||
better virtual address function.
|
||||
|
||||
the collision search can perhaps be optimized - as we search chains we can
|
||||
come across other candidate intersections at other levels, perhaps we should
|
||||
do the intersection check straight away? --> save on list searching time only,
|
||||
which is not too significant.
|
||||
|
||||
*/
|
||||
|
||||
//****************************************************************************
|
||||
|
||||
#include <ode/common.h>
|
||||
#include <ode/space.h>
|
||||
#include <ode/geom.h>
|
||||
#include <ode/error.h>
|
||||
#include <ode/memory.h>
|
||||
#include "objects.h"
|
||||
#include "geom_internal.h"
|
||||
|
||||
//****************************************************************************
|
||||
// space base class
|
||||
|
||||
struct dxSpace : public dBase {
|
||||
int type; // don't want to use RTTI
|
||||
virtual void destroy()=0;
|
||||
virtual void add (dGeomID)=0;
|
||||
virtual void remove (dGeomID)=0;
|
||||
virtual void collide (void *data, dNearCallback *callback)=0;
|
||||
virtual int query (dGeomID)=0;
|
||||
};
|
||||
|
||||
#define TYPE_SIMPLE 0xbad
|
||||
#define TYPE_HASH 0xbabe
|
||||
|
||||
//****************************************************************************
|
||||
// stuff common to all spaces
|
||||
|
||||
#define ALLOCA(x) dALLOCA16(x)
|
||||
|
||||
|
||||
// collide two AABBs together. for the hash table space, this is called if
|
||||
// the two AABBs inhabit the same hash table cells. this only calls the
|
||||
// callback function if the boxes actually intersect. if a geom has an
|
||||
// AABB test function, that is called to provide a further refinement of
|
||||
// the intersection.
|
||||
|
||||
static inline void collideAABBs (dReal bounds1[6], dReal bounds2[6],
|
||||
dxGeom *g1, dxGeom *g2,
|
||||
void *data, dNearCallback *callback)
|
||||
{
|
||||
// no contacts if both geoms on the same body, and the body is not 0
|
||||
if (g1->body == g2->body && g1->body) return;
|
||||
|
||||
if (bounds1[0] > bounds2[1] ||
|
||||
bounds1[1] < bounds2[0] ||
|
||||
bounds1[2] > bounds2[3] ||
|
||||
bounds1[3] < bounds2[2] ||
|
||||
bounds1[4] > bounds2[5] ||
|
||||
bounds1[5] < bounds2[4]) return;
|
||||
if (g1->_class->aabb_test) {
|
||||
if (g1->_class->aabb_test (g1,g2,bounds2) == 0) return;
|
||||
}
|
||||
if (g2->_class->aabb_test) {
|
||||
if (g2->_class->aabb_test (g2,g1,bounds1) == 0) return;
|
||||
}
|
||||
callback (data,g1,g2);
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// simple space - reports all n^2 object intersections
|
||||
|
||||
struct dxSimpleSpace : public dxSpace {
|
||||
dGeomID first;
|
||||
void destroy();
|
||||
void add (dGeomID);
|
||||
void remove (dGeomID);
|
||||
void collide (void *data, dNearCallback *callback);
|
||||
int query (dGeomID);
|
||||
};
|
||||
|
||||
|
||||
dSpaceID dSimpleSpaceCreate()
|
||||
{
|
||||
dxSimpleSpace *w = new dxSimpleSpace;
|
||||
w->type = TYPE_SIMPLE;
|
||||
w->first = 0;
|
||||
return w;
|
||||
}
|
||||
|
||||
|
||||
void dxSimpleSpace::destroy()
|
||||
{
|
||||
// destroying each geom will call remove(). this will be efficient if
|
||||
// we destroy geoms in list order.
|
||||
dAASSERT (this);
|
||||
dGeomID g,n;
|
||||
g = first;
|
||||
while (g) {
|
||||
n = g->space.next;
|
||||
dGeomDestroy (g);
|
||||
g = n;
|
||||
}
|
||||
delete this;
|
||||
}
|
||||
|
||||
|
||||
void dxSimpleSpace::add (dGeomID obj)
|
||||
{
|
||||
dAASSERT (this && obj);
|
||||
dUASSERT (obj->spaceid == 0 && obj->space.next == 0,
|
||||
"object is already in a space");
|
||||
obj->space.next = first;
|
||||
first = obj;
|
||||
obj->spaceid = this;
|
||||
}
|
||||
|
||||
|
||||
void dxSimpleSpace::remove (dGeomID geom_to_remove)
|
||||
{
|
||||
dAASSERT (this && geom_to_remove);
|
||||
dUASSERT (geom_to_remove->spaceid,"object is not in a space");
|
||||
dGeomID last=0,g=first;
|
||||
while (g) {
|
||||
if (g==geom_to_remove) {
|
||||
if (last) last->space.next = g->space.next;
|
||||
else first = g->space.next;
|
||||
geom_to_remove->space.next = 0;
|
||||
geom_to_remove->spaceid = 0;
|
||||
return;
|
||||
}
|
||||
last = g;
|
||||
g = g->space.next;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dxSimpleSpace::collide (void *data, dNearCallback *callback)
|
||||
{
|
||||
dAASSERT (this && callback);
|
||||
dxGeom *g1,*g2;
|
||||
int i,j,n;
|
||||
|
||||
// count the number of objects
|
||||
n=0;
|
||||
for (g1=first; g1; g1=g1->space.next) n++;
|
||||
|
||||
// allocate and fill bounds array
|
||||
dReal *bounds = (dReal*) ALLOCA (6 * n * sizeof(dReal));
|
||||
i=0;
|
||||
for (g1=first; g1; g1=g1->space.next) {
|
||||
g1->_class->aabb (g1,bounds + i);
|
||||
g1->space_aabb = bounds + i;
|
||||
i += 6;
|
||||
}
|
||||
|
||||
// intersect all bounding boxes
|
||||
i=0;
|
||||
for (g1=first; g1; g1=g1->space.next) {
|
||||
j=i+6;
|
||||
for (g2=g1->space.next; g2; g2=g2->space.next) {
|
||||
collideAABBs (bounds+i,bounds+j,g1,g2,data,callback);
|
||||
j += 6;
|
||||
}
|
||||
i += 6;
|
||||
}
|
||||
|
||||
// reset the aabb fields of the geoms back to 0
|
||||
for (g1=first; g1; g1=g1->space.next) g1->space_aabb = 0;
|
||||
}
|
||||
|
||||
|
||||
// @@@ NOT FLEXIBLE ENOUGH
|
||||
//
|
||||
//int dSpaceCollide (dSpaceID space, dContactGeom **contact_array)
|
||||
//{
|
||||
// int n = 0;
|
||||
// dContactGeom *base = (dContact*) dStackAlloc (sizeof(dContact));
|
||||
// dContactGeom *c = base;
|
||||
// for (dxGeom *g1=space->first; g1; g1=g1->space.next) {
|
||||
// for (dxGeom *g2=g1->space.next; g2; g2=g2->space.next) {
|
||||
// // generate at most 1 contact for this pair
|
||||
// c->o1 = g1;
|
||||
// c->o2 = g2;
|
||||
// if (dCollide (0,c)) {
|
||||
// c = (dContactGeom*) dStackAlloc (sizeof(dContactGeom));
|
||||
// n++;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// *contact_array = base;
|
||||
// return n;
|
||||
//}
|
||||
|
||||
|
||||
int dxSimpleSpace::query (dGeomID obj)
|
||||
{
|
||||
dAASSERT (this && obj);
|
||||
if (obj->spaceid != this) return 0;
|
||||
dGeomID compare = first;
|
||||
while (compare) {
|
||||
if (compare == obj) return 1;
|
||||
compare = compare->space.next;
|
||||
}
|
||||
dDebug (0,"object is not in the space it thinks it is in");
|
||||
return 0;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// hash table space
|
||||
|
||||
// kind of silly, but oh well...
|
||||
#define MAXINT ((int)((((unsigned int)(-1)) << 1) >> 1))
|
||||
|
||||
|
||||
// prime[i] is the largest prime smaller than 2^i
|
||||
#define NUM_PRIMES 31
|
||||
static long int prime[NUM_PRIMES] = {1L,2L,3L,7L,13L,31L,61L,127L,251L,509L,
|
||||
1021L,2039L,4093L,8191L,16381L,32749L,65521L,131071L,262139L,
|
||||
524287L,1048573L,2097143L,4194301L,8388593L,16777213L,33554393L,
|
||||
67108859L,134217689L,268435399L,536870909L,1073741789L};
|
||||
|
||||
|
||||
// currently the space 'container' is just a list of the geoms in the space.
|
||||
|
||||
struct dxHashSpace : public dxSpace {
|
||||
dxGeom *first;
|
||||
int global_minlevel; // smallest hash table level to put AABBs in
|
||||
int global_maxlevel; // objects that need a level larger than this will be
|
||||
// put in a "big objects" list instead of a hash table
|
||||
void destroy();
|
||||
void add (dGeomID);
|
||||
void remove (dGeomID);
|
||||
void collide (void *data, dNearCallback *callback);
|
||||
int query (dGeomID);
|
||||
};
|
||||
|
||||
|
||||
// an axis aligned bounding box
|
||||
struct dxAABB {
|
||||
dxAABB *next; // next in the list of all AABBs
|
||||
dReal bounds[6]; // minx, maxx, miny, maxy, minz, maxz
|
||||
int level; // the level this is stored in (cell size = 2^level)
|
||||
int dbounds[6]; // AABB bounds, discretized to cell size
|
||||
dxGeom *geom; // corresponding geometry object
|
||||
int index; // index of this AABB, starting from 0
|
||||
};
|
||||
|
||||
|
||||
// a hash table node that represents an AABB that intersects a particular cell
|
||||
// at a particular level
|
||||
struct Node {
|
||||
Node *next; // next node in hash table collision list, 0 if none
|
||||
int x,y,z; // cell position in space, discretized to cell size
|
||||
dxAABB *aabb; // axis aligned bounding box that intersects this cell
|
||||
};
|
||||
|
||||
|
||||
// return the `level' of an AABB. the AABB will be put into cells at this
|
||||
// level - the cell size will be 2^level. the level is chosen to be the
|
||||
// smallest value such that the AABB occupies no more than 8 cells, regardless
|
||||
// of its placement. this means that:
|
||||
// size/2 < q <= size
|
||||
// where q is the maximum AABB dimension.
|
||||
|
||||
static int findLevel (dReal bounds[6])
|
||||
{
|
||||
// compute q
|
||||
dReal q,q2;
|
||||
q = bounds[1] - bounds[0]; // x bounds
|
||||
q2 = bounds[3] - bounds[2]; // y bounds
|
||||
if (q2 > q) q = q2;
|
||||
q2 = bounds[5] - bounds[4]; // z bounds
|
||||
if (q2 > q) q = q2;
|
||||
|
||||
if (q == dInfinity) return MAXINT;
|
||||
|
||||
// find level such that 0.5 * 2^level < q <= 2^level
|
||||
int level;
|
||||
frexp (q,&level); // q = (0.5 .. 1.0) * 2^level (definition of frexp)
|
||||
return level;
|
||||
}
|
||||
|
||||
|
||||
// find a virtual memory address for a cell at the given level and x,y,z
|
||||
// position.
|
||||
// @@@ currently this is not very sophisticated, e.g. the scaling
|
||||
// factors could be better designed to avoid collisions, and they should
|
||||
// probably depend on the hash table physical size.
|
||||
|
||||
static unsigned long getVirtualAddress (int level, int x, int y, int z)
|
||||
{
|
||||
return level*1000 + x*100 + y*10 + z;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// hash space public functions
|
||||
|
||||
dSpaceID dHashSpaceCreate()
|
||||
{
|
||||
dxHashSpace *w = new dxHashSpace;
|
||||
w->type = TYPE_HASH;
|
||||
w->first = 0;
|
||||
w->global_minlevel = -3;
|
||||
w->global_maxlevel = 10;
|
||||
return w;
|
||||
}
|
||||
|
||||
|
||||
void dxHashSpace::destroy()
|
||||
{
|
||||
// destroying each geom will call remove(). this will be efficient if
|
||||
// we destroy geoms in list order.
|
||||
dAASSERT (this);
|
||||
dGeomID g,n;
|
||||
g = first;
|
||||
while (g) {
|
||||
n = g->space.next;
|
||||
dGeomDestroy (g);
|
||||
g = n;
|
||||
}
|
||||
delete this;
|
||||
}
|
||||
|
||||
|
||||
void dHashSpaceSetLevels (dxSpace *space, int minlevel, int maxlevel)
|
||||
{
|
||||
dUASSERT (minlevel <= maxlevel,"must have minlevel <= maxlevel");
|
||||
dUASSERT (space->type == TYPE_HASH,"must be a hash space");
|
||||
dxHashSpace *hspace = (dxHashSpace*) space;
|
||||
hspace->global_minlevel = minlevel;
|
||||
hspace->global_maxlevel = maxlevel;
|
||||
}
|
||||
|
||||
|
||||
void dxHashSpace::add (dGeomID obj)
|
||||
{
|
||||
dAASSERT (this && obj);
|
||||
dUASSERT (obj->spaceid == 0 && obj->space.next == 0,
|
||||
"object is already in a space");
|
||||
obj->space.next = first;
|
||||
first = obj;
|
||||
obj->spaceid = this;
|
||||
}
|
||||
|
||||
|
||||
void dxHashSpace::remove (dGeomID geom_to_remove)
|
||||
{
|
||||
dAASSERT (this && geom_to_remove);
|
||||
dUASSERT (geom_to_remove->spaceid,"object is not in a space");
|
||||
dGeomID last=0,g=first;
|
||||
while (g) {
|
||||
if (g==geom_to_remove) {
|
||||
if (last) last->space.next = g->space.next;
|
||||
else first = g->space.next;
|
||||
geom_to_remove->space.next = 0;
|
||||
geom_to_remove->spaceid = 0;
|
||||
return;
|
||||
}
|
||||
last = g;
|
||||
g = g->space.next;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dxHashSpace::collide (void *data, dNearCallback *callback)
|
||||
{
|
||||
dAASSERT(this && callback);
|
||||
dxGeom *geom;
|
||||
dxAABB *aabb;
|
||||
int i,maxlevel;
|
||||
|
||||
// create a list of axis aligned bounding boxes for all geoms. count the
|
||||
// number of AABBs as we go. set the level for all AABBs. put AABBs larger
|
||||
// than the space's global_maxlevel in the big_boxes list, check everything
|
||||
// else against that list at the end. for AABBs that are not too big,
|
||||
// record the maximum level that we need.
|
||||
|
||||
int n = 0; // number of AABBs in main list
|
||||
int ntotal = 0; // total number of AABBs
|
||||
dxAABB *first_aabb = 0; // list of AABBs in hash table
|
||||
dxAABB *big_boxes = 0; // list of AABBs too big for hash table
|
||||
maxlevel = global_minlevel - 1;
|
||||
for (geom = first; geom; geom=geom->space.next) {
|
||||
ntotal++;
|
||||
dxAABB *aabb = (dxAABB*) ALLOCA (sizeof(dxAABB));
|
||||
geom->_class->aabb (geom,aabb->bounds);
|
||||
geom->space_aabb = aabb->bounds;
|
||||
aabb->geom = geom;
|
||||
// compute level, but prevent cells from getting too small
|
||||
int level = findLevel (aabb->bounds);
|
||||
if (level < global_minlevel) level = global_minlevel;
|
||||
if (level <= global_maxlevel) {
|
||||
// aabb goes in main list
|
||||
aabb->next = first_aabb;
|
||||
first_aabb = aabb;
|
||||
aabb->level = level;
|
||||
if (level > maxlevel) maxlevel = level;
|
||||
// cellsize = 2^level
|
||||
dReal cellsize = (dReal) ldexp (1.0,level);
|
||||
// discretize AABB position to cell size
|
||||
for (i=0; i < 6; i++) aabb->dbounds[i] = (int)
|
||||
floor (aabb->bounds[i]/cellsize);
|
||||
// set AABB index
|
||||
aabb->index = n;
|
||||
n++;
|
||||
}
|
||||
else {
|
||||
// aabb is too big, put it in the big_boxes list. we don't care about
|
||||
// setting level, dbounds, index, or the maxlevel
|
||||
aabb->next = big_boxes;
|
||||
big_boxes = aabb;
|
||||
}
|
||||
}
|
||||
|
||||
// 0 or 1 boxes can't collide with anything
|
||||
if (ntotal < 2) return;
|
||||
|
||||
// for `n' objects, an n*n array of bits is used to record if those objects
|
||||
// have been intersection-tested against each other yet. this array can
|
||||
// grow large with high n, but oh well...
|
||||
int tested_rowsize = (n+7) >> 3; // number of bytes needed for n bits
|
||||
unsigned char *tested = (unsigned char *) alloca (n * tested_rowsize);
|
||||
memset (tested,0,n * tested_rowsize);
|
||||
|
||||
// create a hash table to store all AABBs. each AABB may take up to 8 cells.
|
||||
// we use chaining to resolve collisions, but we use a relatively large table
|
||||
// to reduce the chance of collisions.
|
||||
|
||||
// compute hash table size sz to be a prime > 8*n
|
||||
for (i=0; i<NUM_PRIMES; i++) {
|
||||
if (prime[i] >= (8*n)) break;
|
||||
}
|
||||
if (i >= NUM_PRIMES) i = NUM_PRIMES-1; // probably pointless
|
||||
int sz = prime[i];
|
||||
|
||||
// allocate and initialize hash table node pointers
|
||||
Node **table = (Node **) ALLOCA (sizeof(Node*) * sz);
|
||||
for (i=0; i<sz; i++) table[i] = 0;
|
||||
|
||||
// add each AABB to the hash table (may need to add it to up to 8 cells)
|
||||
for (aabb=first_aabb; aabb; aabb=aabb->next) {
|
||||
int *dbounds = aabb->dbounds;
|
||||
for (int xi = dbounds[0]; xi <= dbounds[1]; xi++) {
|
||||
for (int yi = dbounds[2]; yi <= dbounds[3]; yi++) {
|
||||
for (int zi = dbounds[4]; zi <= dbounds[5]; zi++) {
|
||||
// get the hash index
|
||||
unsigned long hi = getVirtualAddress (aabb->level,xi,yi,zi) % sz;
|
||||
// add a new node to the hash table
|
||||
Node *node = (Node*) alloca (sizeof (Node));
|
||||
node->x = xi;
|
||||
node->y = yi;
|
||||
node->z = zi;
|
||||
node->aabb = aabb;
|
||||
node->next = table[hi];
|
||||
table[hi] = node;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// now that all AABBs are loaded into the hash table, we do the actual
|
||||
// collision detection. for all AABBs, check for other AABBs in the
|
||||
// same cells for collisions, and then check for other AABBs in all
|
||||
// intersecting higher level cells.
|
||||
|
||||
int db[6]; // discrete bounds at current level
|
||||
for (aabb=first_aabb; aabb; aabb=aabb->next) {
|
||||
// we are searching for collisions with aabb
|
||||
for (i=0; i<6; i++) db[i] = aabb->dbounds[i];
|
||||
for (int level = aabb->level; level <= maxlevel; level++) {
|
||||
for (int xi = db[0]; xi <= db[1]; xi++) {
|
||||
for (int yi = db[2]; yi <= db[3]; yi++) {
|
||||
for (int zi = db[4]; zi <= db[5]; zi++) {
|
||||
// get the hash index
|
||||
unsigned long hi = getVirtualAddress (level,xi,yi,zi) % sz;
|
||||
// search all nodes at this index
|
||||
Node *node;
|
||||
for (node = table[hi]; node; node=node->next) {
|
||||
// node points to an AABB that may intersect aabb
|
||||
if (node->aabb == aabb) continue;
|
||||
if (node->aabb->level == level &&
|
||||
node->x == xi && node->y == yi && node->z == zi) {
|
||||
// see if aabb and node->aabb have already been tested
|
||||
// against each other
|
||||
unsigned char mask;
|
||||
if (aabb->index <= node->aabb->index) {
|
||||
i = (aabb->index * tested_rowsize)+(node->aabb->index >> 3);
|
||||
mask = 1 << (node->aabb->index & 7);
|
||||
}
|
||||
else {
|
||||
i = (node->aabb->index * tested_rowsize)+(aabb->index >> 3);
|
||||
mask = 1 << (aabb->index & 7);
|
||||
}
|
||||
dIASSERT (i >= 0 && i < (tested_rowsize*n));
|
||||
if ((tested[i] & mask)==0) {
|
||||
collideAABBs (aabb->bounds,node->aabb->bounds,
|
||||
aabb->geom,node->aabb->geom,
|
||||
data,callback);
|
||||
}
|
||||
tested[i] |= mask;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// get the discrete bounds for the next level up
|
||||
for (i=0; i<6; i++) db[i] >>= 1;
|
||||
}
|
||||
}
|
||||
|
||||
// every AABB in the normal list must now be intersected against every
|
||||
// AABB in the big_boxes list. so let's hope there are not too many objects
|
||||
// in the big_boxes list.
|
||||
for (aabb=first_aabb; aabb; aabb=aabb->next) {
|
||||
for (dxAABB *aabb2=big_boxes; aabb2; aabb2=aabb2->next) {
|
||||
collideAABBs (aabb->bounds,aabb2->bounds,aabb->geom,aabb2->geom,
|
||||
data,callback);
|
||||
}
|
||||
}
|
||||
|
||||
// intersected all AABBs in the big_boxes list together
|
||||
for (aabb=big_boxes; aabb; aabb=aabb->next) {
|
||||
for (dxAABB *aabb2=aabb->next; aabb2; aabb2=aabb2->next) {
|
||||
collideAABBs (aabb->bounds,aabb2->bounds,aabb->geom,aabb2->geom,
|
||||
data,callback);
|
||||
}
|
||||
}
|
||||
|
||||
// reset the aabb fields of the geoms back to 0
|
||||
for (geom=first; geom; geom=geom->space.next) geom->space_aabb = 0;
|
||||
}
|
||||
|
||||
|
||||
int dxHashSpace::query (dGeomID obj)
|
||||
{
|
||||
dAASSERT (this && obj);
|
||||
if (obj->spaceid != this) return 0;
|
||||
dGeomID compare = first;
|
||||
while (compare) {
|
||||
if (compare == obj) return 1;
|
||||
compare = compare->space.next;
|
||||
}
|
||||
dDebug (0,"object is not in the space it thinks it is in");
|
||||
return 0;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// space functions
|
||||
|
||||
void dSpaceDestroy (dxSpace * space)
|
||||
{
|
||||
space->destroy();
|
||||
}
|
||||
|
||||
|
||||
void dSpaceAdd (dxSpace * space, dxGeom *g)
|
||||
{
|
||||
space->add (g);
|
||||
}
|
||||
|
||||
|
||||
void dSpaceRemove (dxSpace * space, dxGeom *g)
|
||||
{
|
||||
space->remove (g);
|
||||
}
|
||||
|
||||
|
||||
int dSpaceQuery (dxSpace * space, dxGeom *g)
|
||||
{
|
||||
return space->query (g);
|
||||
}
|
||||
|
||||
|
||||
void dSpaceCollide (dxSpace * space, void *data, dNearCallback *callback)
|
||||
{
|
||||
space->collide (data,callback);
|
||||
}
|
||||
114
extern/ode/dist/ode/src/stack.cpp
vendored
114
extern/ode/dist/ode/src/stack.cpp
vendored
@@ -1,114 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
@@@ this file should not be compiled any more @@@
|
||||
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include "stack.h"
|
||||
#include "ode/error.h"
|
||||
#include "ode/config.h"
|
||||
|
||||
//****************************************************************************
|
||||
// unix version that uses mmap(). some systems have anonymous mmaps and some
|
||||
// need to mmap /dev/zero.
|
||||
|
||||
#ifndef WIN32
|
||||
|
||||
#include <unistd.h>
|
||||
#include <sys/mman.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
|
||||
void dStack::init (int max_size)
|
||||
{
|
||||
if (sizeof(long int) != sizeof(char*)) dDebug (0,"internal");
|
||||
if (max_size <= 0) dDebug (0,"Stack::init() given size <= 0");
|
||||
|
||||
#ifndef MMAP_ANONYMOUS
|
||||
static int dev_zero_fd = -1; // cached file descriptor for /dev/zero
|
||||
if (dev_zero_fd < 0) dev_zero_fd = open ("/dev/zero", O_RDWR);
|
||||
if (dev_zero_fd < 0) dError (0,"can't open /dev/zero (%s)",strerror(errno));
|
||||
base = (char*) mmap (0,max_size, PROT_READ | PROT_WRITE, MAP_PRIVATE,
|
||||
dev_zero_fd,0);
|
||||
#else
|
||||
base = (char*) mmap (0,max_size, PROT_READ | PROT_WRITE,
|
||||
MAP_PRIVATE | MAP_ANON,0,0);
|
||||
#endif
|
||||
|
||||
if (int(base) == -1) dError (0,"Stack::init(), mmap() failed, "
|
||||
"max_size=%d (%s)",max_size,strerror(errno));
|
||||
size = max_size;
|
||||
pointer = base;
|
||||
frame = 0;
|
||||
}
|
||||
|
||||
|
||||
void dStack::destroy()
|
||||
{
|
||||
munmap (base,size);
|
||||
base = 0;
|
||||
size = 0;
|
||||
pointer = 0;
|
||||
frame = 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
//****************************************************************************
|
||||
|
||||
#ifdef WIN32
|
||||
|
||||
#include "windows.h"
|
||||
|
||||
|
||||
void dStack::init (int max_size)
|
||||
{
|
||||
if (sizeof(LPVOID) != sizeof(char*)) dDebug (0,"internal");
|
||||
if (max_size <= 0) dDebug (0,"Stack::init() given size <= 0");
|
||||
base = (char*) VirtualAlloc (NULL,max_size,MEM_RESERVE,PAGE_READWRITE);
|
||||
if (base == 0) dError (0,"Stack::init(), VirtualAlloc() failed, "
|
||||
"max_size=%d",max_size);
|
||||
size = max_size;
|
||||
pointer = base;
|
||||
frame = 0;
|
||||
committed = 0;
|
||||
|
||||
// get page size
|
||||
SYSTEM_INFO info;
|
||||
GetSystemInfo (&info);
|
||||
pagesize = info.dwPageSize;
|
||||
}
|
||||
|
||||
|
||||
void dStack::destroy()
|
||||
{
|
||||
VirtualFree (base,0,MEM_RELEASE);
|
||||
base = 0;
|
||||
size = 0;
|
||||
pointer = 0;
|
||||
frame = 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
139
extern/ode/dist/ode/src/stack.h
vendored
139
extern/ode/dist/ode/src/stack.h
vendored
@@ -1,139 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
/* this comes from the `reuse' library. copy any changes back to the source.
|
||||
|
||||
these stack allocation functions are a replacement for alloca(), except that
|
||||
they allocate memory from a separate pool.
|
||||
|
||||
advantages over alloca():
|
||||
- consecutive allocations are guaranteed to be contiguous with increasing
|
||||
address.
|
||||
- functions can allocate stack memory that is returned to the caller,
|
||||
in other words pushing and popping stack frames is optional.
|
||||
|
||||
disadvantages compared to alloca():
|
||||
- less portable
|
||||
- slightly slower, although still orders of magnitude faster than malloc().
|
||||
- longjmp() and exceptions do not deallocate stack memory (but who cares?).
|
||||
|
||||
just like alloca():
|
||||
- using too much stack memory does not fail gracefully, it fails with a
|
||||
segfault.
|
||||
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _ODE_STACK_H_
|
||||
#define _ODE_STACK_H_
|
||||
|
||||
|
||||
#ifdef WIN32
|
||||
#include "windows.h"
|
||||
#endif
|
||||
|
||||
|
||||
struct dStack {
|
||||
char *base; // bottom of the stack
|
||||
int size; // maximum size of the stack
|
||||
char *pointer; // current top of the stack
|
||||
char *frame; // linked list of stack frame ptrs
|
||||
# ifdef WIN32 // stuff for windows:
|
||||
int pagesize; // - page size - this is ASSUMED to be a power of 2
|
||||
int committed; // - bytes committed in allocated region
|
||||
#endif
|
||||
|
||||
// initialize the stack. `max_size' is the maximum size that the stack can
|
||||
// reach. on unix and windows a `virtual' memory block of this size is
|
||||
// mapped into the address space but does not actually consume physical
|
||||
// memory until it is referenced - so it is safe to set this to a high value.
|
||||
|
||||
void init (int max_size);
|
||||
|
||||
|
||||
// destroy the stack. this unmaps any virtual memory that was allocated.
|
||||
|
||||
void destroy();
|
||||
|
||||
|
||||
// allocate `size' bytes from the stack and return a pointer to the allocated
|
||||
// memory. `size' must be >= 0. the returned pointer will be aligned to the
|
||||
// size of a long int.
|
||||
|
||||
char * alloc (int size)
|
||||
{
|
||||
char *ret = pointer;
|
||||
pointer += ((size-1) | (sizeof(long int)-1) )+1;
|
||||
# ifdef WIN32
|
||||
// for windows we need to commit pages as they are required
|
||||
if ((pointer-base) > committed) {
|
||||
committed = ((pointer-base-1) | (pagesize-1))+1; // round up to pgsize
|
||||
VirtualAlloc (base,committed,MEM_COMMIT,PAGE_READWRITE);
|
||||
}
|
||||
# endif
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
// return the address that will be returned by the next call to alloc()
|
||||
|
||||
char *nextAlloc()
|
||||
{
|
||||
return pointer;
|
||||
}
|
||||
|
||||
|
||||
// push and pop the current size of the stack. pushFrame() saves the current
|
||||
// frame pointer on the stack, and popFrame() retrieves it. a typical
|
||||
// stack-using function will bracket alloc() calls with pushFrame() and
|
||||
// popFrame(). both functions return the current stack pointer - this should
|
||||
// be the same value for the two bracketing calls. calling popFrame() too
|
||||
// many times will result in a segfault.
|
||||
|
||||
char * pushFrame()
|
||||
{
|
||||
char *newframe = pointer;
|
||||
char **addr = (char**) alloc (sizeof(char*));
|
||||
*addr = frame;
|
||||
frame = newframe;
|
||||
return newframe;
|
||||
|
||||
/* OLD CODE
|
||||
*((char**)pointer) = frame;
|
||||
frame = pointer;
|
||||
char *ret = pointer;
|
||||
pointer += sizeof(char*);
|
||||
return ret;
|
||||
*/
|
||||
}
|
||||
|
||||
char * popFrame()
|
||||
{
|
||||
pointer = frame;
|
||||
frame = *((char**)pointer);
|
||||
return pointer;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
1085
extern/ode/dist/ode/src/step.cpp
vendored
1085
extern/ode/dist/ode/src/step.cpp
vendored
@@ -1,1085 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#include "objects.h"
|
||||
#include "joint.h"
|
||||
#include <ode/config.h>
|
||||
#include <ode/odemath.h>
|
||||
#include <ode/rotation.h>
|
||||
#include <ode/timer.h>
|
||||
#include <ode/error.h>
|
||||
#include <ode/matrix.h>
|
||||
#include "lcp.h"
|
||||
|
||||
//****************************************************************************
|
||||
// misc defines
|
||||
|
||||
#define FAST_FACTOR
|
||||
//#define TIMING
|
||||
|
||||
#define ALLOCA dALLOCA16
|
||||
|
||||
//****************************************************************************
|
||||
// debugging - comparison of various vectors and matrices produced by the
|
||||
// slow and fast versions of the stepper.
|
||||
|
||||
//#define COMPARE_METHODS
|
||||
|
||||
#ifdef COMPARE_METHODS
|
||||
#include "testing.h"
|
||||
dMatrixComparison comparator;
|
||||
#endif
|
||||
|
||||
//****************************************************************************
|
||||
// special matrix multipliers
|
||||
|
||||
// this assumes the 4th and 8th rows of B and C are zero.
|
||||
|
||||
static void Multiply2_p8r (dReal *A, dReal *B, dReal *C,
|
||||
int p, int r, int Askip)
|
||||
{
|
||||
int i,j;
|
||||
dReal sum,*bb,*cc;
|
||||
dIASSERT (p>0 && r>0 && A && B && C);
|
||||
bb = B;
|
||||
for (i=p; i; i--) {
|
||||
cc = C;
|
||||
for (j=r; j; j--) {
|
||||
sum = bb[0]*cc[0];
|
||||
sum += bb[1]*cc[1];
|
||||
sum += bb[2]*cc[2];
|
||||
sum += bb[4]*cc[4];
|
||||
sum += bb[5]*cc[5];
|
||||
sum += bb[6]*cc[6];
|
||||
*(A++) = sum;
|
||||
cc += 8;
|
||||
}
|
||||
A += Askip - r;
|
||||
bb += 8;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// this assumes the 4th and 8th rows of B and C are zero.
|
||||
|
||||
static void MultiplyAdd2_p8r (dReal *A, dReal *B, dReal *C,
|
||||
int p, int r, int Askip)
|
||||
{
|
||||
int i,j;
|
||||
dReal sum,*bb,*cc;
|
||||
dIASSERT (p>0 && r>0 && A && B && C);
|
||||
bb = B;
|
||||
for (i=p; i; i--) {
|
||||
cc = C;
|
||||
for (j=r; j; j--) {
|
||||
sum = bb[0]*cc[0];
|
||||
sum += bb[1]*cc[1];
|
||||
sum += bb[2]*cc[2];
|
||||
sum += bb[4]*cc[4];
|
||||
sum += bb[5]*cc[5];
|
||||
sum += bb[6]*cc[6];
|
||||
*(A++) += sum;
|
||||
cc += 8;
|
||||
}
|
||||
A += Askip - r;
|
||||
bb += 8;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// this assumes the 4th and 8th rows of B are zero.
|
||||
|
||||
static void Multiply0_p81 (dReal *A, dReal *B, dReal *C, int p)
|
||||
{
|
||||
int i;
|
||||
dIASSERT (p>0 && A && B && C);
|
||||
dReal sum;
|
||||
for (i=p; i; i--) {
|
||||
sum = B[0]*C[0];
|
||||
sum += B[1]*C[1];
|
||||
sum += B[2]*C[2];
|
||||
sum += B[4]*C[4];
|
||||
sum += B[5]*C[5];
|
||||
sum += B[6]*C[6];
|
||||
*(A++) = sum;
|
||||
B += 8;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// this assumes the 4th and 8th rows of B are zero.
|
||||
|
||||
static void MultiplyAdd0_p81 (dReal *A, dReal *B, dReal *C, int p)
|
||||
{
|
||||
int i;
|
||||
dIASSERT (p>0 && A && B && C);
|
||||
dReal sum;
|
||||
for (i=p; i; i--) {
|
||||
sum = B[0]*C[0];
|
||||
sum += B[1]*C[1];
|
||||
sum += B[2]*C[2];
|
||||
sum += B[4]*C[4];
|
||||
sum += B[5]*C[5];
|
||||
sum += B[6]*C[6];
|
||||
*(A++) += sum;
|
||||
B += 8;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// this assumes the 4th and 8th rows of B are zero.
|
||||
|
||||
static void MultiplyAdd1_8q1 (dReal *A, dReal *B, dReal *C, int q)
|
||||
{
|
||||
int k;
|
||||
dReal sum;
|
||||
dIASSERT (q>0 && A && B && C);
|
||||
sum = 0;
|
||||
for (k=0; k<q; k++) sum += B[k*8] * C[k];
|
||||
A[0] += sum;
|
||||
sum = 0;
|
||||
for (k=0; k<q; k++) sum += B[1+k*8] * C[k];
|
||||
A[1] += sum;
|
||||
sum = 0;
|
||||
for (k=0; k<q; k++) sum += B[2+k*8] * C[k];
|
||||
A[2] += sum;
|
||||
sum = 0;
|
||||
for (k=0; k<q; k++) sum += B[4+k*8] * C[k];
|
||||
A[4] += sum;
|
||||
sum = 0;
|
||||
for (k=0; k<q; k++) sum += B[5+k*8] * C[k];
|
||||
A[5] += sum;
|
||||
sum = 0;
|
||||
for (k=0; k<q; k++) sum += B[6+k*8] * C[k];
|
||||
A[6] += sum;
|
||||
}
|
||||
|
||||
|
||||
// this assumes the 4th and 8th rows of B are zero.
|
||||
|
||||
static void Multiply1_8q1 (dReal *A, dReal *B, dReal *C, int q)
|
||||
{
|
||||
int k;
|
||||
dReal sum;
|
||||
dIASSERT (q>0 && A && B && C);
|
||||
sum = 0;
|
||||
for (k=0; k<q; k++) sum += B[k*8] * C[k];
|
||||
A[0] = sum;
|
||||
sum = 0;
|
||||
for (k=0; k<q; k++) sum += B[1+k*8] * C[k];
|
||||
A[1] = sum;
|
||||
sum = 0;
|
||||
for (k=0; k<q; k++) sum += B[2+k*8] * C[k];
|
||||
A[2] = sum;
|
||||
sum = 0;
|
||||
for (k=0; k<q; k++) sum += B[4+k*8] * C[k];
|
||||
A[4] = sum;
|
||||
sum = 0;
|
||||
for (k=0; k<q; k++) sum += B[5+k*8] * C[k];
|
||||
A[5] = sum;
|
||||
sum = 0;
|
||||
for (k=0; k<q; k++) sum += B[6+k*8] * C[k];
|
||||
A[6] = sum;
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// body rotation
|
||||
|
||||
// return sin(x)/x. this has a singularity at 0 so special handling is needed
|
||||
// for small arguments.
|
||||
|
||||
static inline dReal sinc (dReal x)
|
||||
{
|
||||
// if |x| < 1e-4 then use a taylor series expansion. this two term expansion
|
||||
// is actually accurate to one LS bit within this range if double precision
|
||||
// is being used - so don't worry!
|
||||
if (dFabs(x) < 1.0e-4) return REAL(1.0) - x*x*REAL(0.166666666666666666667);
|
||||
else return dSin(x)/x;
|
||||
}
|
||||
|
||||
|
||||
// given a body b, apply its linear and angular rotation over the time
|
||||
// interval h, thereby adjusting its position and orientation.
|
||||
|
||||
static inline void moveAndRotateBody (dxBody *b, dReal h)
|
||||
{
|
||||
int j;
|
||||
|
||||
// handle linear velocity
|
||||
for (j=0; j<3; j++) b->pos[j] += h * b->lvel[j];
|
||||
|
||||
if (b->flags & dxBodyFlagFiniteRotation) {
|
||||
dVector3 irv; // infitesimal rotation vector
|
||||
dQuaternion q; // quaternion for finite rotation
|
||||
|
||||
if (b->flags & dxBodyFlagFiniteRotationAxis) {
|
||||
// split the angular velocity vector into a component along the finite
|
||||
// rotation axis, and a component orthogonal to it.
|
||||
dVector3 frv,irv; // finite rotation vector
|
||||
dReal k = dDOT (b->finite_rot_axis,b->avel);
|
||||
frv[0] = b->finite_rot_axis[0] * k;
|
||||
frv[1] = b->finite_rot_axis[1] * k;
|
||||
frv[2] = b->finite_rot_axis[2] * k;
|
||||
irv[0] = b->avel[0] - frv[0];
|
||||
irv[1] = b->avel[1] - frv[1];
|
||||
irv[2] = b->avel[2] - frv[2];
|
||||
|
||||
// make a rotation quaternion q that corresponds to frv * h.
|
||||
// compare this with the full-finite-rotation case below.
|
||||
h *= REAL(0.5);
|
||||
dReal theta = k * h;
|
||||
q[0] = dCos(theta);
|
||||
dReal s = sinc(theta) * h;
|
||||
q[1] = frv[0] * s;
|
||||
q[2] = frv[1] * s;
|
||||
q[3] = frv[2] * s;
|
||||
}
|
||||
else {
|
||||
// make a rotation quaternion q that corresponds to w * h
|
||||
dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] +
|
||||
b->avel[2]*b->avel[2]);
|
||||
h *= REAL(0.5);
|
||||
dReal theta = wlen * h;
|
||||
q[0] = dCos(theta);
|
||||
dReal s = sinc(theta) * h;
|
||||
q[1] = b->avel[0] * s;
|
||||
q[2] = b->avel[1] * s;
|
||||
q[3] = b->avel[2] * s;
|
||||
}
|
||||
|
||||
// do the finite rotation
|
||||
dQuaternion q2;
|
||||
dQMultiply0 (q2,q,b->q);
|
||||
for (j=0; j<4; j++) b->q[j] = q2[j];
|
||||
|
||||
// do the infitesimal rotation if required
|
||||
if (b->flags & dxBodyFlagFiniteRotationAxis) {
|
||||
dReal dq[4];
|
||||
dWtoDQ (irv,b->q,dq);
|
||||
for (j=0; j<4; j++) b->q[j] += h * dq[j];
|
||||
}
|
||||
}
|
||||
else {
|
||||
// the normal way - do an infitesimal rotation
|
||||
dReal dq[4];
|
||||
dWtoDQ (b->avel,b->q,dq);
|
||||
for (j=0; j<4; j++) b->q[j] += h * dq[j];
|
||||
}
|
||||
|
||||
// normalize the quaternion and convert it to a rotation matrix
|
||||
dNormalize4 (b->q);
|
||||
dQtoR (b->q,b->R);
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// the slow, but sure way
|
||||
// note that this does not do any joint feedback!
|
||||
|
||||
// given lists of bodies and joints that form an island, perform a first
|
||||
// order timestep.
|
||||
//
|
||||
// `body' is the body array, `nb' is the size of the array.
|
||||
// `_joint' is the body array, `nj' is the size of the array.
|
||||
|
||||
void dInternalStepIsland_x1 (dxWorld *world, dxBody * const *body, int nb,
|
||||
dxJoint * const *_joint, int nj, dReal stepsize)
|
||||
{
|
||||
int i,j,k;
|
||||
int n6 = 6*nb;
|
||||
|
||||
# ifdef TIMING
|
||||
dTimerStart("preprocessing");
|
||||
# endif
|
||||
|
||||
// number all bodies in the body list - set their tag values
|
||||
for (i=0; i<nb; i++) body[i]->tag = i;
|
||||
|
||||
// make a local copy of the joint array, because we might want to modify it.
|
||||
// (the "dxJoint *const*" declaration says we're allowed to modify the joints
|
||||
// but not the joint array, because the caller might need it unchanged).
|
||||
dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*));
|
||||
memcpy (joint,_joint,nj * sizeof(dxJoint*));
|
||||
|
||||
// for all bodies, compute the inertia tensor and its inverse in the global
|
||||
// frame, and compute the rotational force and add it to the torque
|
||||
// accumulator.
|
||||
// @@@ check computation of rotational force.
|
||||
dReal *I = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
|
||||
dReal *invI = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
|
||||
dSetZero (I,3*nb*4);
|
||||
dSetZero (invI,3*nb*4);
|
||||
for (i=0; i<nb; i++) {
|
||||
dReal tmp[12];
|
||||
// compute inertia tensor in global frame
|
||||
dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->R);
|
||||
dMULTIPLY0_333 (I+i*12,body[i]->R,tmp);
|
||||
// compute inverse inertia tensor in global frame
|
||||
dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R);
|
||||
dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp);
|
||||
// compute rotational force
|
||||
dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
|
||||
dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
|
||||
}
|
||||
|
||||
// add the gravity force to all bodies
|
||||
for (i=0; i<nb; i++) {
|
||||
if ((body[i]->flags & dxBodyNoGravity)==0) {
|
||||
body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
|
||||
body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
|
||||
body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
|
||||
}
|
||||
}
|
||||
|
||||
// get m = total constraint dimension, nub = number of unbounded variables.
|
||||
// create constraint offset array and number-of-rows array for all joints.
|
||||
// the constraints are re-ordered as follows: the purely unbounded
|
||||
// constraints, the mixed unbounded + LCP constraints, and last the purely
|
||||
// LCP constraints.
|
||||
//
|
||||
// joints with m=0 are inactive and are removed from the joints array
|
||||
// entirely, so that the code that follows does not consider them.
|
||||
int m = 0;
|
||||
dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1));
|
||||
int *ofs = (int*) ALLOCA (nj*sizeof(int));
|
||||
for (i=0, j=0; j<nj; j++) { // i=dest, j=src
|
||||
joint[j]->vtable->getInfo1 (joint[j],info+i);
|
||||
dIASSERT (info[i].m >= 0 && info[i].m <= 6 &&
|
||||
info[i].nub >= 0 && info[i].nub <= info[i].m);
|
||||
if (info[i].m > 0) {
|
||||
joint[i] = joint[j];
|
||||
i++;
|
||||
}
|
||||
}
|
||||
nj = i;
|
||||
|
||||
// the purely unbounded constraints
|
||||
for (i=0; i<nj; i++) if (info[i].nub == info[i].m) {
|
||||
ofs[i] = m;
|
||||
m += info[i].m;
|
||||
}
|
||||
int nub = m;
|
||||
// the mixed unbounded + LCP constraints
|
||||
for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) {
|
||||
ofs[i] = m;
|
||||
m += info[i].m;
|
||||
}
|
||||
// the purely LCP constraints
|
||||
for (i=0; i<nj; i++) if (info[i].nub == 0) {
|
||||
ofs[i] = m;
|
||||
m += info[i].m;
|
||||
}
|
||||
|
||||
// create (6*nb,6*nb) inverse mass matrix `invM', and fill it with mass
|
||||
// parameters
|
||||
# ifdef TIMING
|
||||
dTimerNow ("create mass matrix");
|
||||
# endif
|
||||
int nskip = dPAD (n6);
|
||||
dReal *invM = (dReal*) ALLOCA (n6*nskip*sizeof(dReal));
|
||||
dSetZero (invM,n6*nskip);
|
||||
for (i=0; i<nb; i++) {
|
||||
dReal *MM = invM+(i*6)*nskip+(i*6);
|
||||
MM[0] = body[i]->invMass;
|
||||
MM[nskip+1] = body[i]->invMass;
|
||||
MM[2*nskip+2] = body[i]->invMass;
|
||||
MM += 3*nskip+3;
|
||||
for (j=0; j<3; j++) for (k=0; k<3; k++) {
|
||||
MM[j*nskip+k] = invI[i*12+j*4+k];
|
||||
}
|
||||
}
|
||||
|
||||
// assemble some body vectors: fe = external forces, v = velocities
|
||||
dReal *fe = (dReal*) ALLOCA (n6 * sizeof(dReal));
|
||||
dReal *v = (dReal*) ALLOCA (n6 * sizeof(dReal));
|
||||
dSetZero (fe,n6);
|
||||
dSetZero (v,n6);
|
||||
for (i=0; i<nb; i++) {
|
||||
for (j=0; j<3; j++) fe[i*6+j] = body[i]->facc[j];
|
||||
for (j=0; j<3; j++) fe[i*6+3+j] = body[i]->tacc[j];
|
||||
for (j=0; j<3; j++) v[i*6+j] = body[i]->lvel[j];
|
||||
for (j=0; j<3; j++) v[i*6+3+j] = body[i]->avel[j];
|
||||
}
|
||||
|
||||
// this will be set to the velocity update
|
||||
dReal *vnew = (dReal*) ALLOCA (n6 * sizeof(dReal));
|
||||
dSetZero (vnew,n6);
|
||||
|
||||
// if there are constraints, compute cforce
|
||||
if (m > 0) {
|
||||
// create a constraint equation right hand side vector `c', a constraint
|
||||
// force mixing vector `cfm', and LCP low and high bound vectors, and an
|
||||
// 'findex' vector.
|
||||
dReal *c = (dReal*) ALLOCA (m*sizeof(dReal));
|
||||
dReal *cfm = (dReal*) ALLOCA (m*sizeof(dReal));
|
||||
dReal *lo = (dReal*) ALLOCA (m*sizeof(dReal));
|
||||
dReal *hi = (dReal*) ALLOCA (m*sizeof(dReal));
|
||||
int *findex = (int*) alloca (m*sizeof(int));
|
||||
dSetZero (c,m);
|
||||
dSetValue (cfm,m,world->global_cfm);
|
||||
dSetValue (lo,m,-dInfinity);
|
||||
dSetValue (hi,m, dInfinity);
|
||||
for (i=0; i<m; i++) findex[i] = -1;
|
||||
|
||||
// create (m,6*nb) jacobian mass matrix `J', and fill it with constraint
|
||||
// data. also fill the c vector.
|
||||
# ifdef TIMING
|
||||
dTimerNow ("create J");
|
||||
# endif
|
||||
dReal *J = (dReal*) ALLOCA (m*nskip*sizeof(dReal));
|
||||
dSetZero (J,m*nskip);
|
||||
dxJoint::Info2 Jinfo;
|
||||
Jinfo.rowskip = nskip;
|
||||
Jinfo.fps = dRecip(stepsize);
|
||||
Jinfo.erp = world->global_erp;
|
||||
for (i=0; i<nj; i++) {
|
||||
Jinfo.J1l = J + nskip*ofs[i] + 6*joint[i]->node[0].body->tag;
|
||||
Jinfo.J1a = Jinfo.J1l + 3;
|
||||
if (joint[i]->node[1].body) {
|
||||
Jinfo.J2l = J + nskip*ofs[i] + 6*joint[i]->node[1].body->tag;
|
||||
Jinfo.J2a = Jinfo.J2l + 3;
|
||||
}
|
||||
else {
|
||||
Jinfo.J2l = 0;
|
||||
Jinfo.J2a = 0;
|
||||
}
|
||||
Jinfo.c = c + ofs[i];
|
||||
Jinfo.cfm = cfm + ofs[i];
|
||||
Jinfo.lo = lo + ofs[i];
|
||||
Jinfo.hi = hi + ofs[i];
|
||||
Jinfo.findex = findex + ofs[i];
|
||||
joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
|
||||
// adjust returned findex values for global index numbering
|
||||
for (j=0; j<info[i].m; j++) {
|
||||
if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
|
||||
}
|
||||
}
|
||||
|
||||
// compute A = J*invM*J'
|
||||
# ifdef TIMING
|
||||
dTimerNow ("compute A");
|
||||
# endif
|
||||
dReal *JinvM = (dReal*) ALLOCA (m*nskip*sizeof(dReal));
|
||||
dSetZero (JinvM,m*nskip);
|
||||
dMultiply0 (JinvM,J,invM,m,n6,n6);
|
||||
int mskip = dPAD(m);
|
||||
dReal *A = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
|
||||
dSetZero (A,m*mskip);
|
||||
dMultiply2 (A,JinvM,J,m,n6,m);
|
||||
|
||||
// add cfm to the diagonal of A
|
||||
for (i=0; i<m; i++) A[i*mskip+i] += cfm[i] * Jinfo.fps;
|
||||
|
||||
# ifdef COMPARE_METHODS
|
||||
comparator.nextMatrix (A,m,m,1,"A");
|
||||
# endif
|
||||
|
||||
// compute `rhs', the right hand side of the equation J*a=c
|
||||
# ifdef TIMING
|
||||
dTimerNow ("compute rhs");
|
||||
# endif
|
||||
dReal *tmp1 = (dReal*) ALLOCA (n6 * sizeof(dReal));
|
||||
dSetZero (tmp1,n6);
|
||||
dMultiply0 (tmp1,invM,fe,n6,n6,1);
|
||||
for (i=0; i<n6; i++) tmp1[i] += v[i]/stepsize;
|
||||
dReal *rhs = (dReal*) ALLOCA (m * sizeof(dReal));
|
||||
dSetZero (rhs,m);
|
||||
dMultiply0 (rhs,J,tmp1,m,n6,1);
|
||||
for (i=0; i<m; i++) rhs[i] = c[i]/stepsize - rhs[i];
|
||||
|
||||
# ifdef COMPARE_METHODS
|
||||
comparator.nextMatrix (c,m,1,0,"c");
|
||||
comparator.nextMatrix (rhs,m,1,0,"rhs");
|
||||
# endif
|
||||
|
||||
// solve the LCP problem and get lambda.
|
||||
// this will destroy A but that's okay
|
||||
# ifdef TIMING
|
||||
dTimerNow ("solving LCP problem");
|
||||
# endif
|
||||
dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
|
||||
dReal *residual = (dReal*) ALLOCA (m * sizeof(dReal));
|
||||
dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex);
|
||||
|
||||
// OLD WAY - direct factor and solve
|
||||
//
|
||||
// // factorize A (L*L'=A)
|
||||
//# ifdef TIMING
|
||||
// dTimerNow ("factorize A");
|
||||
//# endif
|
||||
// dReal *L = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
|
||||
// memcpy (L,A,m*mskip*sizeof(dReal));
|
||||
// if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite");
|
||||
//
|
||||
// // compute lambda
|
||||
//# ifdef TIMING
|
||||
// dTimerNow ("compute lambda");
|
||||
//# endif
|
||||
// dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
|
||||
// memcpy (lambda,rhs,m * sizeof(dReal));
|
||||
// dSolveCholesky (L,lambda,m);
|
||||
|
||||
# ifdef COMPARE_METHODS
|
||||
comparator.nextMatrix (lambda,m,1,0,"lambda");
|
||||
# endif
|
||||
|
||||
// compute the velocity update `vnew'
|
||||
# ifdef TIMING
|
||||
dTimerNow ("compute velocity update");
|
||||
# endif
|
||||
dMultiply1 (tmp1,J,lambda,n6,m,1);
|
||||
for (i=0; i<n6; i++) tmp1[i] += fe[i];
|
||||
dMultiply0 (vnew,invM,tmp1,n6,n6,1);
|
||||
for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i];
|
||||
|
||||
// see if the constraint has worked: compute J*vnew and make sure it equals
|
||||
// `c' (to within a certain tolerance).
|
||||
# ifdef TIMING
|
||||
dTimerNow ("verify constraint equation");
|
||||
# endif
|
||||
dMultiply0 (tmp1,J,vnew,m,n6,1);
|
||||
dReal err = 0;
|
||||
for (i=0; i<m; i++) err += dFabs(tmp1[i]-c[i]);
|
||||
printf ("%.6e\n",err);
|
||||
}
|
||||
else {
|
||||
// no constraints
|
||||
dMultiply0 (vnew,invM,fe,n6,n6,1);
|
||||
for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i];
|
||||
}
|
||||
|
||||
# ifdef COMPARE_METHODS
|
||||
comparator.nextMatrix (vnew,n6,1,0,"vnew");
|
||||
# endif
|
||||
|
||||
// apply the velocity update to the bodies
|
||||
# ifdef TIMING
|
||||
dTimerNow ("update velocity");
|
||||
# endif
|
||||
for (i=0; i<nb; i++) {
|
||||
for (j=0; j<3; j++) body[i]->lvel[j] = vnew[i*6+j];
|
||||
for (j=0; j<3; j++) body[i]->avel[j] = vnew[i*6+3+j];
|
||||
}
|
||||
|
||||
// update the position and orientation from the new linear/angular velocity
|
||||
// (over the given timestep)
|
||||
# ifdef TIMING
|
||||
dTimerNow ("update position");
|
||||
# endif
|
||||
for (i=0; i<nb; i++) moveAndRotateBody (body[i],stepsize);
|
||||
|
||||
# ifdef TIMING
|
||||
dTimerNow ("tidy up");
|
||||
# endif
|
||||
|
||||
// zero all force accumulators
|
||||
for (i=0; i<nb; i++) {
|
||||
body[i]->facc[0] = 0;
|
||||
body[i]->facc[1] = 0;
|
||||
body[i]->facc[2] = 0;
|
||||
body[i]->facc[3] = 0;
|
||||
body[i]->tacc[0] = 0;
|
||||
body[i]->tacc[1] = 0;
|
||||
body[i]->tacc[2] = 0;
|
||||
body[i]->tacc[3] = 0;
|
||||
}
|
||||
|
||||
# ifdef TIMING
|
||||
dTimerEnd();
|
||||
if (m > 0) dTimerReport (stdout,1);
|
||||
# endif
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// an optimized version of dInternalStepIsland1()
|
||||
|
||||
void dInternalStepIsland_x2 (dxWorld *world, dxBody * const *body, int nb,
|
||||
dxJoint * const *_joint, int nj, dReal stepsize)
|
||||
{
|
||||
int i,j,k;
|
||||
# ifdef TIMING
|
||||
dTimerStart("preprocessing");
|
||||
# endif
|
||||
|
||||
dReal stepsize1 = dRecip(stepsize);
|
||||
|
||||
// number all bodies in the body list - set their tag values
|
||||
for (i=0; i<nb; i++) body[i]->tag = i;
|
||||
|
||||
// make a local copy of the joint array, because we might want to modify it.
|
||||
// (the "dxJoint *const*" declaration says we're allowed to modify the joints
|
||||
// but not the joint array, because the caller might need it unchanged).
|
||||
dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*));
|
||||
memcpy (joint,_joint,nj * sizeof(dxJoint*));
|
||||
|
||||
// for all bodies, compute the inertia tensor and its inverse in the global
|
||||
// frame, and compute the rotational force and add it to the torque
|
||||
// accumulator. I and invI are vertically stacked 3x4 matrices, one per body.
|
||||
// @@@ check computation of rotational force.
|
||||
dReal *I = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
|
||||
dReal *invI = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
|
||||
dSetZero (I,3*nb*4);
|
||||
dSetZero (invI,3*nb*4);
|
||||
for (i=0; i<nb; i++) {
|
||||
dReal tmp[12];
|
||||
// compute inertia tensor in global frame
|
||||
dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->R);
|
||||
dMULTIPLY0_333 (I+i*12,body[i]->R,tmp);
|
||||
// compute inverse inertia tensor in global frame
|
||||
dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R);
|
||||
dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp);
|
||||
// compute rotational force
|
||||
dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
|
||||
dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
|
||||
}
|
||||
|
||||
// add the gravity force to all bodies
|
||||
for (i=0; i<nb; i++) {
|
||||
if ((body[i]->flags & dxBodyNoGravity)==0) {
|
||||
body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
|
||||
body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
|
||||
body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
|
||||
}
|
||||
}
|
||||
|
||||
// get m = total constraint dimension, nub = number of unbounded variables.
|
||||
// create constraint offset array and number-of-rows array for all joints.
|
||||
// the constraints are re-ordered as follows: the purely unbounded
|
||||
// constraints, the mixed unbounded + LCP constraints, and last the purely
|
||||
// LCP constraints. this assists the LCP solver to put all unbounded
|
||||
// variables at the start for a quick factorization.
|
||||
//
|
||||
// joints with m=0 are inactive and are removed from the joints array
|
||||
// entirely, so that the code that follows does not consider them.
|
||||
// also number all active joints in the joint list (set their tag values).
|
||||
// inactive joints receive a tag value of -1.
|
||||
|
||||
int m = 0;
|
||||
dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1));
|
||||
int *ofs = (int*) ALLOCA (nj*sizeof(int));
|
||||
for (i=0, j=0; j<nj; j++) { // i=dest, j=src
|
||||
joint[j]->vtable->getInfo1 (joint[j],info+i);
|
||||
dIASSERT (info[i].m >= 0 && info[i].m <= 6 &&
|
||||
info[i].nub >= 0 && info[i].nub <= info[i].m);
|
||||
if (info[i].m > 0) {
|
||||
joint[i] = joint[j];
|
||||
joint[i]->tag = i;
|
||||
i++;
|
||||
}
|
||||
else {
|
||||
joint[j]->tag = -1;
|
||||
}
|
||||
}
|
||||
nj = i;
|
||||
|
||||
// the purely unbounded constraints
|
||||
for (i=0; i<nj; i++) if (info[i].nub == info[i].m) {
|
||||
ofs[i] = m;
|
||||
m += info[i].m;
|
||||
}
|
||||
int nub = m;
|
||||
// the mixed unbounded + LCP constraints
|
||||
for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) {
|
||||
ofs[i] = m;
|
||||
m += info[i].m;
|
||||
}
|
||||
// the purely LCP constraints
|
||||
for (i=0; i<nj; i++) if (info[i].nub == 0) {
|
||||
ofs[i] = m;
|
||||
m += info[i].m;
|
||||
}
|
||||
|
||||
// this will be set to the force due to the constraints
|
||||
dReal *cforce = (dReal*) ALLOCA (nb*8 * sizeof(dReal));
|
||||
dSetZero (cforce,nb*8);
|
||||
|
||||
// if there are constraints, compute cforce
|
||||
if (m > 0) {
|
||||
// create a constraint equation right hand side vector `c', a constraint
|
||||
// force mixing vector `cfm', and LCP low and high bound vectors, and an
|
||||
// 'findex' vector.
|
||||
dReal *c = (dReal*) ALLOCA (m*sizeof(dReal));
|
||||
dReal *cfm = (dReal*) ALLOCA (m*sizeof(dReal));
|
||||
dReal *lo = (dReal*) ALLOCA (m*sizeof(dReal));
|
||||
dReal *hi = (dReal*) ALLOCA (m*sizeof(dReal));
|
||||
int *findex = (int*) alloca (m*sizeof(int));
|
||||
dSetZero (c,m);
|
||||
dSetValue (cfm,m,world->global_cfm);
|
||||
dSetValue (lo,m,-dInfinity);
|
||||
dSetValue (hi,m, dInfinity);
|
||||
for (i=0; i<m; i++) findex[i] = -1;
|
||||
|
||||
// get jacobian data from constraints. a (2*m)x8 matrix will be created
|
||||
// to store the two jacobian blocks from each constraint. it has this
|
||||
// format:
|
||||
//
|
||||
// l l l 0 a a a 0 \
|
||||
// l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows)
|
||||
// l l l 0 a a a 0 /
|
||||
// l l l 0 a a a 0 \
|
||||
// l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows)
|
||||
// l l l 0 a a a 0 /
|
||||
// l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row)
|
||||
// l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row)
|
||||
// etc...
|
||||
//
|
||||
// (lll) = linear jacobian data
|
||||
// (aaa) = angular jacobian data
|
||||
//
|
||||
# ifdef TIMING
|
||||
dTimerNow ("create J");
|
||||
# endif
|
||||
dReal *J = (dReal*) ALLOCA (2*m*8*sizeof(dReal));
|
||||
dSetZero (J,2*m*8);
|
||||
dxJoint::Info2 Jinfo;
|
||||
Jinfo.rowskip = 8;
|
||||
Jinfo.fps = stepsize1;
|
||||
Jinfo.erp = world->global_erp;
|
||||
for (i=0; i<nj; i++) {
|
||||
Jinfo.J1l = J + 2*8*ofs[i];
|
||||
Jinfo.J1a = Jinfo.J1l + 4;
|
||||
Jinfo.J2l = Jinfo.J1l + 8*info[i].m;
|
||||
Jinfo.J2a = Jinfo.J2l + 4;
|
||||
Jinfo.c = c + ofs[i];
|
||||
Jinfo.cfm = cfm + ofs[i];
|
||||
Jinfo.lo = lo + ofs[i];
|
||||
Jinfo.hi = hi + ofs[i];
|
||||
Jinfo.findex = findex + ofs[i];
|
||||
joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
|
||||
// adjust returned findex values for global index numbering
|
||||
for (j=0; j<info[i].m; j++) {
|
||||
if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
|
||||
}
|
||||
}
|
||||
|
||||
// compute A = J*invM*J'. first compute JinvM = J*invM. this has the same
|
||||
// format as J so we just go through the constraints in J multiplying by
|
||||
// the appropriate scalars and matrices.
|
||||
# ifdef TIMING
|
||||
dTimerNow ("compute A");
|
||||
# endif
|
||||
dReal *JinvM = (dReal*) ALLOCA (2*m*8*sizeof(dReal));
|
||||
dSetZero (JinvM,2*m*8);
|
||||
for (i=0; i<nj; i++) {
|
||||
int b = joint[i]->node[0].body->tag;
|
||||
dReal body_invMass = body[b]->invMass;
|
||||
dReal *body_invI = invI + b*12;
|
||||
dReal *Jsrc = J + 2*8*ofs[i];
|
||||
dReal *Jdst = JinvM + 2*8*ofs[i];
|
||||
for (j=info[i].m-1; j>=0; j--) {
|
||||
for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass;
|
||||
dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI);
|
||||
Jsrc += 8;
|
||||
Jdst += 8;
|
||||
}
|
||||
if (joint[i]->node[1].body) {
|
||||
b = joint[i]->node[1].body->tag;
|
||||
body_invMass = body[b]->invMass;
|
||||
body_invI = invI + b*12;
|
||||
for (j=info[i].m-1; j>=0; j--) {
|
||||
for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass;
|
||||
dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI);
|
||||
Jsrc += 8;
|
||||
Jdst += 8;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// now compute A = JinvM * J'. A's rows and columns are grouped by joint,
|
||||
// i.e. in the same way as the rows of J. block (i,j) of A is only nonzero
|
||||
// if joints i and j have at least one body in common. this fact suggests
|
||||
// the algorithm used to fill A:
|
||||
//
|
||||
// for b = all bodies
|
||||
// n = number of joints attached to body b
|
||||
// for i = 1..n
|
||||
// for j = i+1..n
|
||||
// ii = actual joint number for i
|
||||
// jj = actual joint number for j
|
||||
// // (ii,jj) will be set to all pairs of joints around body b
|
||||
// compute blockwise: A(ii,jj) += JinvM(ii) * J(jj)'
|
||||
//
|
||||
// this algorithm catches all pairs of joints that have at least one body
|
||||
// in common. it does not compute the diagonal blocks of A however -
|
||||
// another similar algorithm does that.
|
||||
|
||||
int mskip = dPAD(m);
|
||||
dReal *A = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
|
||||
dSetZero (A,m*mskip);
|
||||
for (i=0; i<nb; i++) {
|
||||
for (dxJointNode *n1=body[i]->firstjoint; n1; n1=n1->next) {
|
||||
for (dxJointNode *n2=n1->next; n2; n2=n2->next) {
|
||||
// get joint numbers and ensure ofs[j1] >= ofs[j2]
|
||||
int j1 = n1->joint->tag;
|
||||
int j2 = n2->joint->tag;
|
||||
if (ofs[j1] < ofs[j2]) {
|
||||
int tmp = j1;
|
||||
j1 = j2;
|
||||
j2 = tmp;
|
||||
}
|
||||
|
||||
// if either joint was tagged as -1 then it is an inactive (m=0)
|
||||
// joint that should not be considered
|
||||
if (j1==-1 || j2==-1) continue;
|
||||
|
||||
// determine if body i is the 1st or 2nd body of joints j1 and j2
|
||||
int jb1 = (joint[j1]->node[1].body == body[i]);
|
||||
int jb2 = (joint[j2]->node[1].body == body[i]);
|
||||
// jb1/jb2 must be 0 for joints with only one body
|
||||
dIASSERT(joint[j1]->node[1].body || jb1==0);
|
||||
dIASSERT(joint[j2]->node[1].body || jb2==0);
|
||||
|
||||
// set block of A
|
||||
MultiplyAdd2_p8r (A + ofs[j1]*mskip + ofs[j2],
|
||||
JinvM + 2*8*ofs[j1] + jb1*8*info[j1].m,
|
||||
J + 2*8*ofs[j2] + jb2*8*info[j2].m,
|
||||
info[j1].m,info[j2].m, mskip);
|
||||
}
|
||||
}
|
||||
}
|
||||
// compute diagonal blocks of A
|
||||
for (i=0; i<nj; i++) {
|
||||
Multiply2_p8r (A + ofs[i]*(mskip+1),
|
||||
JinvM + 2*8*ofs[i],
|
||||
J + 2*8*ofs[i],
|
||||
info[i].m,info[i].m, mskip);
|
||||
if (joint[i]->node[1].body) {
|
||||
MultiplyAdd2_p8r (A + ofs[i]*(mskip+1),
|
||||
JinvM + 2*8*ofs[i] + 8*info[i].m,
|
||||
J + 2*8*ofs[i] + 8*info[i].m,
|
||||
info[i].m,info[i].m, mskip);
|
||||
}
|
||||
}
|
||||
|
||||
// add cfm to the diagonal of A
|
||||
for (i=0; i<m; i++) A[i*mskip+i] += cfm[i] * stepsize1;
|
||||
|
||||
# ifdef COMPARE_METHODS
|
||||
comparator.nextMatrix (A,m,m,1,"A");
|
||||
# endif
|
||||
|
||||
// compute the right hand side `rhs'
|
||||
# ifdef TIMING
|
||||
dTimerNow ("compute rhs");
|
||||
# endif
|
||||
dReal *tmp1 = (dReal*) ALLOCA (nb*8 * sizeof(dReal));
|
||||
dSetZero (tmp1,nb*8);
|
||||
// put v/h + invM*fe into tmp1
|
||||
for (i=0; i<nb; i++) {
|
||||
dReal body_invMass = body[i]->invMass;
|
||||
dReal *body_invI = invI + i*12;
|
||||
for (j=0; j<3; j++) tmp1[i*8+j] = body[i]->facc[j] * body_invMass +
|
||||
body[i]->lvel[j] * stepsize1;
|
||||
dMULTIPLY0_331 (tmp1 + i*8 + 4,body_invI,body[i]->tacc);
|
||||
for (j=0; j<3; j++) tmp1[i*8+4+j] += body[i]->avel[j] * stepsize1;
|
||||
}
|
||||
// put J*tmp1 into rhs
|
||||
dReal *rhs = (dReal*) ALLOCA (m * sizeof(dReal));
|
||||
dSetZero (rhs,m);
|
||||
for (i=0; i<nj; i++) {
|
||||
dReal *JJ = J + 2*8*ofs[i];
|
||||
Multiply0_p81 (rhs+ofs[i],JJ,
|
||||
tmp1 + 8*joint[i]->node[0].body->tag, info[i].m);
|
||||
if (joint[i]->node[1].body) {
|
||||
MultiplyAdd0_p81 (rhs+ofs[i],JJ + 8*info[i].m,
|
||||
tmp1 + 8*joint[i]->node[1].body->tag, info[i].m);
|
||||
}
|
||||
}
|
||||
// complete rhs
|
||||
for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];
|
||||
|
||||
# ifdef COMPARE_METHODS
|
||||
comparator.nextMatrix (c,m,1,0,"c");
|
||||
comparator.nextMatrix (rhs,m,1,0,"rhs");
|
||||
# endif
|
||||
|
||||
// solve the LCP problem and get lambda.
|
||||
// this will destroy A but that's okay
|
||||
# ifdef TIMING
|
||||
dTimerNow ("solving LCP problem");
|
||||
# endif
|
||||
dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
|
||||
dReal *residual = (dReal*) ALLOCA (m * sizeof(dReal));
|
||||
dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex);
|
||||
|
||||
// OLD WAY - direct factor and solve
|
||||
//
|
||||
// // factorize A (L*L'=A)
|
||||
//# ifdef TIMING
|
||||
// dTimerNow ("factorize A");
|
||||
//# endif
|
||||
// dReal *L = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
|
||||
// memcpy (L,A,m*mskip*sizeof(dReal));
|
||||
//# ifdef FAST_FACTOR
|
||||
// dFastFactorCholesky (L,m); // does not report non positive definiteness
|
||||
//# else
|
||||
// if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite");
|
||||
//# endif
|
||||
//
|
||||
// // compute lambda
|
||||
//# ifdef TIMING
|
||||
// dTimerNow ("compute lambda");
|
||||
//# endif
|
||||
// dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
|
||||
// memcpy (lambda,rhs,m * sizeof(dReal));
|
||||
// dSolveCholesky (L,lambda,m);
|
||||
|
||||
# ifdef COMPARE_METHODS
|
||||
comparator.nextMatrix (lambda,m,1,0,"lambda");
|
||||
# endif
|
||||
|
||||
// compute the constraint force `cforce'
|
||||
# ifdef TIMING
|
||||
dTimerNow ("compute constraint force");
|
||||
# endif
|
||||
// compute cforce = J'*lambda
|
||||
for (i=0; i<nj; i++) {
|
||||
dReal *JJ = J + 2*8*ofs[i];
|
||||
dxBody* b1 = joint[i]->node[0].body;
|
||||
dxBody* b2 = joint[i]->node[1].body;
|
||||
dJointFeedback *fb = joint[i]->feedback;
|
||||
|
||||
if (fb) {
|
||||
// the user has requested feedback on the amount of force that this
|
||||
// joint is applying to the bodies. we use a slightly slower
|
||||
// computation that splits out the force components and puts them
|
||||
// in the feedback structure.
|
||||
dReal data1[8],data2[8];
|
||||
Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m);
|
||||
dReal *cf1 = cforce + 8*b1->tag;
|
||||
cf1[0] += (fb->f1[0] = data1[0]);
|
||||
cf1[1] += (fb->f1[1] = data1[1]);
|
||||
cf1[2] += (fb->f1[2] = data1[2]);
|
||||
cf1[4] += (fb->t1[0] = data1[4]);
|
||||
cf1[5] += (fb->t1[1] = data1[5]);
|
||||
cf1[6] += (fb->t1[2] = data1[6]);
|
||||
if (b2){
|
||||
Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
|
||||
dReal *cf2 = cforce + 8*b2->tag;
|
||||
cf2[0] += (fb->f2[0] = data2[0]);
|
||||
cf2[1] += (fb->f2[1] = data2[1]);
|
||||
cf2[2] += (fb->f2[2] = data2[2]);
|
||||
cf2[4] += (fb->t2[0] = data2[4]);
|
||||
cf2[5] += (fb->t2[1] = data2[5]);
|
||||
cf2[6] += (fb->t2[2] = data2[6]);
|
||||
}
|
||||
}
|
||||
else {
|
||||
// no feedback is required, let's compute cforce the faster way
|
||||
MultiplyAdd1_8q1 (cforce + 8*b1->tag,JJ, lambda+ofs[i], info[i].m);
|
||||
if (b2) {
|
||||
MultiplyAdd1_8q1 (cforce + 8*b2->tag,
|
||||
JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// compute the velocity update
|
||||
# ifdef TIMING
|
||||
dTimerNow ("compute velocity update");
|
||||
# endif
|
||||
|
||||
// add fe to cforce
|
||||
for (i=0; i<nb; i++) {
|
||||
for (j=0; j<3; j++) cforce[i*8+j] += body[i]->facc[j];
|
||||
for (j=0; j<3; j++) cforce[i*8+4+j] += body[i]->tacc[j];
|
||||
}
|
||||
// multiply cforce by stepsize
|
||||
for (i=0; i < nb*8; i++) cforce[i] *= stepsize;
|
||||
// add invM * cforce to the body velocity
|
||||
for (i=0; i<nb; i++) {
|
||||
dReal body_invMass = body[i]->invMass;
|
||||
dReal *body_invI = invI + i*12;
|
||||
for (j=0; j<3; j++) body[i]->lvel[j] += body_invMass * cforce[i*8+j];
|
||||
dMULTIPLYADD0_331 (body[i]->avel,body_invI,cforce+i*8+4);
|
||||
}
|
||||
|
||||
// update the position and orientation from the new linear/angular velocity
|
||||
// (over the given timestep)
|
||||
# ifdef TIMING
|
||||
dTimerNow ("update position");
|
||||
# endif
|
||||
for (i=0; i<nb; i++) moveAndRotateBody (body[i],stepsize);
|
||||
|
||||
# ifdef COMPARE_METHODS
|
||||
dReal *tmp_vnew = (dReal*) ALLOCA (nb*6*sizeof(dReal));
|
||||
for (i=0; i<nb; i++) {
|
||||
for (j=0; j<3; j++) tmp_vnew[i*6+j] = body[i]->lvel[j];
|
||||
for (j=0; j<3; j++) tmp_vnew[i*6+3+j] = body[i]->avel[j];
|
||||
}
|
||||
comparator.nextMatrix (tmp_vnew,nb*6,1,0,"vnew");
|
||||
# endif
|
||||
|
||||
# ifdef TIMING
|
||||
dTimerNow ("tidy up");
|
||||
# endif
|
||||
|
||||
// zero all force accumulators
|
||||
for (i=0; i<nb; i++) {
|
||||
body[i]->facc[0] = 0;
|
||||
body[i]->facc[1] = 0;
|
||||
body[i]->facc[2] = 0;
|
||||
body[i]->facc[3] = 0;
|
||||
body[i]->tacc[0] = 0;
|
||||
body[i]->tacc[1] = 0;
|
||||
body[i]->tacc[2] = 0;
|
||||
body[i]->tacc[3] = 0;
|
||||
}
|
||||
|
||||
# ifdef TIMING
|
||||
dTimerEnd();
|
||||
if (m > 0) dTimerReport (stdout,1);
|
||||
# endif
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
|
||||
void dInternalStepIsland (dxWorld *world, dxBody * const *body, int nb,
|
||||
dxJoint * const *joint, int nj, dReal stepsize)
|
||||
{
|
||||
# ifndef COMPARE_METHODS
|
||||
dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize);
|
||||
# endif
|
||||
|
||||
# ifdef COMPARE_METHODS
|
||||
int i;
|
||||
|
||||
// save body state
|
||||
dxBody *state = (dxBody*) ALLOCA (nb*sizeof(dxBody));
|
||||
for (i=0; i<nb; i++) memcpy (state+i,body[i],sizeof(dxBody));
|
||||
|
||||
// take slow step
|
||||
comparator.reset();
|
||||
dInternalStepIsland_x1 (world,body,nb,joint,nj,stepsize);
|
||||
comparator.end();
|
||||
|
||||
// restore state
|
||||
for (i=0; i<nb; i++) memcpy (body[i],state+i,sizeof(dxBody));
|
||||
|
||||
// take fast step
|
||||
dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize);
|
||||
comparator.end();
|
||||
|
||||
//comparator.dump();
|
||||
//_exit (1);
|
||||
# endif
|
||||
}
|
||||
37
extern/ode/dist/ode/src/step.h
vendored
37
extern/ode/dist/ode/src/step.h
vendored
@@ -1,37 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#ifndef _ODE_STEP_H_
|
||||
#define _ODE_STEP_H_
|
||||
|
||||
#include <ode/common.h>
|
||||
|
||||
|
||||
void dInternalStepIsland (dxWorld *world,
|
||||
dxBody * const *body, int nb,
|
||||
dxJoint * const *joint, int nj,
|
||||
dReal stepsize);
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
243
extern/ode/dist/ode/src/testing.cpp
vendored
243
extern/ode/dist/ode/src/testing.cpp
vendored
@@ -1,243 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
#include <ode/config.h>
|
||||
#include <ode/misc.h>
|
||||
#include <ode/memory.h>
|
||||
#include "testing.h"
|
||||
|
||||
#ifdef dDOUBLE
|
||||
static const dReal tol = 1.0e-9;
|
||||
#else
|
||||
static const dReal tol = 1.0e-5f;
|
||||
#endif
|
||||
|
||||
|
||||
// matrix header on the stack
|
||||
|
||||
struct dMatrixComparison::dMatInfo {
|
||||
int n,m; // size of matrix
|
||||
char name[128]; // name of the matrix
|
||||
dReal *data; // matrix data
|
||||
int size; // size of `data'
|
||||
};
|
||||
|
||||
|
||||
|
||||
dMatrixComparison::dMatrixComparison()
|
||||
{
|
||||
afterfirst = 0;
|
||||
index = 0;
|
||||
}
|
||||
|
||||
|
||||
dMatrixComparison::~dMatrixComparison()
|
||||
{
|
||||
reset();
|
||||
}
|
||||
|
||||
|
||||
dReal dMatrixComparison::nextMatrix (dReal *A, int n, int m, int lower_tri,
|
||||
char *name, ...)
|
||||
{
|
||||
if (A==0 || n < 1 || m < 1 || name==0) dDebug (0,"bad args to nextMatrix");
|
||||
int num = n*dPAD(m);
|
||||
|
||||
if (afterfirst==0) {
|
||||
dMatInfo *mi = (dMatInfo*) dAlloc (sizeof(dMatInfo));
|
||||
mi->n = n;
|
||||
mi->m = m;
|
||||
mi->size = num * sizeof(dReal);
|
||||
mi->data = (dReal*) dAlloc (mi->size);
|
||||
memcpy (mi->data,A,mi->size);
|
||||
|
||||
va_list ap;
|
||||
va_start (ap,name);
|
||||
vsprintf (mi->name,name,ap);
|
||||
if (strlen(mi->name) >= sizeof (mi->name)) dDebug (0,"name too long");
|
||||
|
||||
mat.push (mi);
|
||||
return 0;
|
||||
}
|
||||
else {
|
||||
if (lower_tri && n != m)
|
||||
dDebug (0,"dMatrixComparison, lower triangular matrix must be square");
|
||||
if (index >= mat.size()) dDebug (0,"dMatrixComparison, too many matrices");
|
||||
dMatInfo *mp = mat[index];
|
||||
index++;
|
||||
|
||||
dMatInfo mi;
|
||||
va_list ap;
|
||||
va_start (ap,name);
|
||||
vsprintf (mi.name,name,ap);
|
||||
if (strlen(mi.name) >= sizeof (mi.name)) dDebug (0,"name too long");
|
||||
|
||||
if (strcmp(mp->name,mi.name) != 0)
|
||||
dDebug (0,"dMatrixComparison, name mismatch (\"%s\" and \"%s\")",
|
||||
mp->name,mi.name);
|
||||
if (mp->n != n || mp->m != m)
|
||||
dDebug (0,"dMatrixComparison, size mismatch (%dx%d and %dx%d)",
|
||||
mp->n,mp->m,n,m);
|
||||
|
||||
dReal maxdiff;
|
||||
if (lower_tri) {
|
||||
maxdiff = dMaxDifferenceLowerTriangle (A,mp->data,n);
|
||||
}
|
||||
else {
|
||||
maxdiff = dMaxDifference (A,mp->data,n,m);
|
||||
}
|
||||
if (maxdiff > tol)
|
||||
dDebug (0,"dMatrixComparison, matrix error (size=%dx%d, name=\"%s\", "
|
||||
"error=%.4e)",n,m,mi.name,maxdiff);
|
||||
return maxdiff;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dMatrixComparison::end()
|
||||
{
|
||||
if (mat.size() <= 0) dDebug (0,"no matrices in sequence");
|
||||
afterfirst = 1;
|
||||
index = 0;
|
||||
}
|
||||
|
||||
|
||||
void dMatrixComparison::reset()
|
||||
{
|
||||
for (int i=0; i<mat.size(); i++) {
|
||||
dFree (mat[i]->data,mat[i]->size);
|
||||
dFree (mat[i],sizeof(dMatInfo));
|
||||
}
|
||||
mat.setSize (0);
|
||||
afterfirst = 0;
|
||||
index = 0;
|
||||
}
|
||||
|
||||
|
||||
void dMatrixComparison::dump()
|
||||
{
|
||||
for (int i=0; i<mat.size(); i++)
|
||||
printf ("%d: %s (%dx%d)\n",i,mat[i]->name,mat[i]->n,mat[i]->m);
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// unit test
|
||||
|
||||
#include <setjmp.h>
|
||||
|
||||
static jmp_buf jump_buffer;
|
||||
|
||||
static void myDebug (int num, const char *msg, va_list ap)
|
||||
{
|
||||
// printf ("(Error %d: ",num);
|
||||
// vprintf (msg,ap);
|
||||
// printf (")\n");
|
||||
longjmp (jump_buffer,1);
|
||||
}
|
||||
|
||||
|
||||
extern "C" void dTestMatrixComparison()
|
||||
{
|
||||
volatile int i;
|
||||
printf ("dTestMatrixComparison()\n");
|
||||
dMessageFunction *orig_debug = dGetDebugHandler();
|
||||
|
||||
dMatrixComparison mc;
|
||||
dReal A[50*50];
|
||||
|
||||
// make first sequence
|
||||
unsigned long seed = dRandGetSeed();
|
||||
for (i=1; i<49; i++) {
|
||||
dMakeRandomMatrix (A,i,i+1,1.0);
|
||||
mc.nextMatrix (A,i,i+1,0,"A%d",i);
|
||||
}
|
||||
mc.end();
|
||||
|
||||
//mc.dump();
|
||||
|
||||
// test identical sequence
|
||||
dSetDebugHandler (&myDebug);
|
||||
dRandSetSeed (seed);
|
||||
if (setjmp (jump_buffer)) {
|
||||
printf ("\tFAILED (1)\n");
|
||||
}
|
||||
else {
|
||||
for (i=1; i<49; i++) {
|
||||
dMakeRandomMatrix (A,i,i+1,1.0);
|
||||
mc.nextMatrix (A,i,i+1,0,"A%d",i);
|
||||
}
|
||||
mc.end();
|
||||
printf ("\tpassed (1)\n");
|
||||
}
|
||||
dSetDebugHandler (orig_debug);
|
||||
|
||||
// test broken sequences (with matrix error)
|
||||
dRandSetSeed (seed);
|
||||
volatile int passcount = 0;
|
||||
for (i=1; i<49; i++) {
|
||||
if (setjmp (jump_buffer)) {
|
||||
passcount++;
|
||||
}
|
||||
else {
|
||||
dSetDebugHandler (&myDebug);
|
||||
dMakeRandomMatrix (A,i,i+1,1.0);
|
||||
A[(i-1)*dPAD(i+1)+i] += REAL(0.01);
|
||||
mc.nextMatrix (A,i,i+1,0,"A%d",i);
|
||||
dSetDebugHandler (orig_debug);
|
||||
}
|
||||
}
|
||||
mc.end();
|
||||
printf ("\t%s (2)\n",(passcount == 48) ? "passed" : "FAILED");
|
||||
|
||||
// test broken sequences (with name error)
|
||||
dRandSetSeed (seed);
|
||||
passcount = 0;
|
||||
for (i=1; i<49; i++) {
|
||||
if (setjmp (jump_buffer)) {
|
||||
passcount++;
|
||||
}
|
||||
else {
|
||||
dSetDebugHandler (&myDebug);
|
||||
dMakeRandomMatrix (A,i,i+1,1.0);
|
||||
mc.nextMatrix (A,i,i+1,0,"B%d",i);
|
||||
dSetDebugHandler (orig_debug);
|
||||
}
|
||||
}
|
||||
mc.end();
|
||||
printf ("\t%s (3)\n",(passcount == 48) ? "passed" : "FAILED");
|
||||
|
||||
// test identical sequence again
|
||||
dSetDebugHandler (&myDebug);
|
||||
dRandSetSeed (seed);
|
||||
if (setjmp (jump_buffer)) {
|
||||
printf ("\tFAILED (4)\n");
|
||||
}
|
||||
else {
|
||||
for (i=1; i<49; i++) {
|
||||
dMakeRandomMatrix (A,i,i+1,1.0);
|
||||
mc.nextMatrix (A,i,i+1,0,"A%d",i);
|
||||
}
|
||||
mc.end();
|
||||
printf ("\tpassed (4)\n");
|
||||
}
|
||||
dSetDebugHandler (orig_debug);
|
||||
}
|
||||
66
extern/ode/dist/ode/src/testing.h
vendored
66
extern/ode/dist/ode/src/testing.h
vendored
@@ -1,66 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
/* stuff used for testing */
|
||||
|
||||
#ifndef _ODE_TESTING_H_
|
||||
#define _ODE_TESTING_H_
|
||||
|
||||
#include <ode/common.h>
|
||||
#include "array.h"
|
||||
|
||||
|
||||
// compare a sequence of named matrices/vectors, i.e. to make sure that two
|
||||
// different pieces of code are giving the same results.
|
||||
|
||||
class dMatrixComparison {
|
||||
struct dMatInfo;
|
||||
dArray<dMatInfo*> mat;
|
||||
int afterfirst,index;
|
||||
|
||||
public:
|
||||
dMatrixComparison();
|
||||
~dMatrixComparison();
|
||||
|
||||
dReal nextMatrix (dReal *A, int n, int m, int lower_tri, char *name, ...);
|
||||
// add a new n*m matrix A to the sequence. the name of the matrix is given
|
||||
// by the printf-style arguments (name,...). if this is the first sequence
|
||||
// then this object will simply record the matrices and return 0.
|
||||
// if this the second or subsequent sequence then this object will compare
|
||||
// the matrices with the first sequence, and report any differences.
|
||||
// the matrix error will be returned. if `lower_tri' is 1 then only the
|
||||
// lower triangle of the matrix (including the diagonal) will be compared
|
||||
// (the matrix must be square).
|
||||
|
||||
void end();
|
||||
// end a sequence.
|
||||
|
||||
void reset();
|
||||
// restarts the object, so the next sequence will be the first sequence.
|
||||
|
||||
void dump();
|
||||
// print out info about all the matrices in the sequence
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
397
extern/ode/dist/ode/src/timer.cpp
vendored
397
extern/ode/dist/ode/src/timer.cpp
vendored
@@ -1,397 +0,0 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||||
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||||
* *
|
||||
* This library is free software; you can redistribute it and/or *
|
||||
* modify it under the terms of EITHER: *
|
||||
* (1) The GNU Lesser General Public License as published by the Free *
|
||||
* Software Foundation; either version 2.1 of the License, or (at *
|
||||
* your option) any later version. The text of the GNU Lesser *
|
||||
* General Public License is included with this library in the *
|
||||
* file LICENSE.TXT. *
|
||||
* (2) The BSD-style license that is included with this library in *
|
||||
* the file LICENSE-BSD.TXT. *
|
||||
* *
|
||||
* This library is distributed in the hope that it will be useful, *
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||||
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||||
* *
|
||||
*************************************************************************/
|
||||
|
||||
/*
|
||||
|
||||
TODO
|
||||
----
|
||||
|
||||
* gettimeofday() and the pentium time stamp counter return the real time,
|
||||
not the process time. fix this somehow!
|
||||
|
||||
*/
|
||||
|
||||
#include <ode/common.h>
|
||||
#include <ode/timer.h>
|
||||
|
||||
// misc defines
|
||||
#define ALLOCA dALLOCA16
|
||||
|
||||
//****************************************************************************
|
||||
// implementation for windows based on the multimedia performance counter.
|
||||
|
||||
#ifdef WIN32
|
||||
|
||||
#include "windows.h"
|
||||
|
||||
static inline void getClockCount (unsigned long cc[2])
|
||||
{
|
||||
LARGE_INTEGER a;
|
||||
QueryPerformanceCounter (&a);
|
||||
cc[0] = a.LowPart;
|
||||
cc[1] = a.HighPart;
|
||||
}
|
||||
|
||||
|
||||
static inline void serialize()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
static inline double loadClockCount (unsigned long cc[2])
|
||||
{
|
||||
LARGE_INTEGER a;
|
||||
a.LowPart = cc[0];
|
||||
a.HighPart = cc[1];
|
||||
return double(a.QuadPart);
|
||||
}
|
||||
|
||||
|
||||
double dTimerResolution()
|
||||
{
|
||||
return 1.0/dTimerTicksPerSecond();
|
||||
}
|
||||
|
||||
|
||||
double dTimerTicksPerSecond()
|
||||
{
|
||||
static int query=0;
|
||||
static double hz=0.0;
|
||||
if (!query) {
|
||||
LARGE_INTEGER a;
|
||||
QueryPerformanceFrequency (&a);
|
||||
hz = double(a.QuadPart);
|
||||
query = 1;
|
||||
}
|
||||
return hz;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
//****************************************************************************
|
||||
// implementation based on the pentium time stamp counter. the timer functions
|
||||
// can be serializing or non-serializing. serializing will ensure that all
|
||||
// instructions have executed and data has been written back before the cpu
|
||||
// time stamp counter is read. the CPUID instruction is used to serialize.
|
||||
|
||||
#if defined(PENTIUM) && !defined(WIN32)
|
||||
|
||||
// we need to know the clock rate so that the timing function can report
|
||||
// accurate times. this number only needs to be set accurately if we're
|
||||
// doing performance tests and care about real-world time numbers - otherwise,
|
||||
// just ignore this. i have not worked out how to determine this number
|
||||
// automatically yet.
|
||||
|
||||
#define PENTIUM_HZ (500e6)
|
||||
|
||||
|
||||
static inline void getClockCount (unsigned long cc[2])
|
||||
{
|
||||
asm volatile ("\n\
|
||||
rdtsc\n\
|
||||
movl %%eax,(%%esi)\n\
|
||||
movl %%edx,4(%%esi)"
|
||||
: : "S" (cc) : "%eax","%edx","cc","memory");
|
||||
}
|
||||
|
||||
|
||||
static inline void serialize()
|
||||
{
|
||||
asm volatile ("\n\
|
||||
mov $0,%%eax\n\
|
||||
cpuid"
|
||||
: : : "%eax","%ebx","%ecx","%edx","cc","memory");
|
||||
}
|
||||
|
||||
|
||||
static inline double loadClockCount (unsigned long a[2])
|
||||
{
|
||||
double ret;
|
||||
asm volatile ("fildll %1; fstpl %0" : "=m" (ret) : "m" (a[0]) :
|
||||
"cc","memory");
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
double dTimerResolution()
|
||||
{
|
||||
return 1.0/PENTIUM_HZ;
|
||||
}
|
||||
|
||||
|
||||
double dTimerTicksPerSecond()
|
||||
{
|
||||
return PENTIUM_HZ;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
//****************************************************************************
|
||||
// otherwise, do the implementation based on gettimeofday().
|
||||
|
||||
#if !defined(PENTIUM) && !defined(WIN32)
|
||||
|
||||
#ifndef macintosh
|
||||
|
||||
#include <sys/time.h>
|
||||
#include <unistd.h>
|
||||
|
||||
|
||||
static inline void getClockCount (unsigned long cc[2])
|
||||
{
|
||||
struct timeval tv;
|
||||
gettimeofday (&tv,0);
|
||||
cc[0] = tv.tv_usec;
|
||||
cc[1] = tv.tv_sec;
|
||||
}
|
||||
|
||||
#else // macintosh
|
||||
|
||||
#include <MacTypes.h>
|
||||
#include <Timer.h>
|
||||
|
||||
static inline void getClockCount (unsigned long cc[2])
|
||||
{
|
||||
UnsignedWide ms;
|
||||
Microseconds (&ms);
|
||||
cc[1] = ms.lo / 1000000;
|
||||
cc[0] = ms.lo - ( cc[1] * 1000000 );
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
static inline void serialize()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
static inline double loadClockCount (unsigned long a[2])
|
||||
{
|
||||
return a[1]*1.0e6 + a[0];
|
||||
}
|
||||
|
||||
|
||||
double dTimerResolution()
|
||||
{
|
||||
unsigned long cc1[2],cc2[2];
|
||||
getClockCount (cc1);
|
||||
do {
|
||||
getClockCount (cc2);
|
||||
}
|
||||
while (cc1[0]==cc2[0] && cc1[1]==cc2[1]);
|
||||
do {
|
||||
getClockCount (cc1);
|
||||
}
|
||||
while (cc1[0]==cc2[0] && cc1[1]==cc2[1]);
|
||||
double t1 = loadClockCount (cc1);
|
||||
double t2 = loadClockCount (cc2);
|
||||
return (t1-t2) / dTimerTicksPerSecond();
|
||||
}
|
||||
|
||||
|
||||
double dTimerTicksPerSecond()
|
||||
{
|
||||
return 1000000;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
//****************************************************************************
|
||||
// stop watches
|
||||
|
||||
void dStopwatchReset (dStopwatch *s)
|
||||
{
|
||||
s->time = 0;
|
||||
s->cc[0] = 0;
|
||||
s->cc[1] = 0;
|
||||
}
|
||||
|
||||
|
||||
void dStopwatchStart (dStopwatch *s)
|
||||
{
|
||||
serialize();
|
||||
getClockCount (s->cc);
|
||||
}
|
||||
|
||||
|
||||
void dStopwatchStop (dStopwatch *s)
|
||||
{
|
||||
unsigned long cc[2];
|
||||
serialize();
|
||||
getClockCount (cc);
|
||||
double t1 = loadClockCount (s->cc);
|
||||
double t2 = loadClockCount (cc);
|
||||
s->time += t2-t1;
|
||||
}
|
||||
|
||||
|
||||
double dStopwatchTime (dStopwatch *s)
|
||||
{
|
||||
return s->time / dTimerTicksPerSecond();
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// code timers
|
||||
|
||||
// maximum number of events to record
|
||||
#define MAXNUM 100
|
||||
|
||||
static int num = 0; // number of entries used in event array
|
||||
static struct {
|
||||
unsigned long cc[2]; // clock counts
|
||||
double total_t; // total clocks used in this slot.
|
||||
double total_p; // total percentage points used in this slot.
|
||||
int count; // number of times this slot has been updated.
|
||||
char *description; // pointer to static string
|
||||
} event[MAXNUM];
|
||||
|
||||
|
||||
// make sure all slot totals and counts reset to 0 at start
|
||||
|
||||
static void initSlots()
|
||||
{
|
||||
static int initialized=0;
|
||||
if (!initialized) {
|
||||
for (int i=0; i<MAXNUM; i++) {
|
||||
event[i].count = 0;
|
||||
event[i].total_t = 0;
|
||||
event[i].total_p = 0;
|
||||
}
|
||||
initialized = 1;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dTimerStart (const char *description)
|
||||
{
|
||||
initSlots();
|
||||
event[0].description = const_cast<char*> (description);
|
||||
num = 1;
|
||||
serialize();
|
||||
getClockCount (event[0].cc);
|
||||
}
|
||||
|
||||
|
||||
void dTimerNow (const char *description)
|
||||
{
|
||||
if (num < MAXNUM) {
|
||||
// do not serialize
|
||||
getClockCount (event[num].cc);
|
||||
event[num].description = const_cast<char*> (description);
|
||||
num++;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dTimerEnd()
|
||||
{
|
||||
if (num < MAXNUM) {
|
||||
serialize();
|
||||
getClockCount (event[num].cc);
|
||||
event[num].description = "TOTAL";
|
||||
num++;
|
||||
}
|
||||
}
|
||||
|
||||
//****************************************************************************
|
||||
// print report
|
||||
|
||||
static void fprintDoubleWithPrefix (FILE *f, double a, char *fmt)
|
||||
{
|
||||
if (a >= 0.999999) {
|
||||
fprintf (f,fmt,a);
|
||||
return;
|
||||
}
|
||||
a *= 1000.0;
|
||||
if (a >= 0.999999) {
|
||||
fprintf (f,fmt,a);
|
||||
fprintf (f,"m");
|
||||
return;
|
||||
}
|
||||
a *= 1000.0;
|
||||
if (a >= 0.999999) {
|
||||
fprintf (f,fmt,a);
|
||||
fprintf (f,"u");
|
||||
return;
|
||||
}
|
||||
a *= 1000.0;
|
||||
fprintf (f,fmt,a);
|
||||
fprintf (f,"n");
|
||||
}
|
||||
|
||||
|
||||
void dTimerReport (FILE *fout, int average)
|
||||
{
|
||||
int i,maxl;
|
||||
double ccunit = 1.0/dTimerTicksPerSecond();
|
||||
fprintf (fout,"\nTimer Report (");
|
||||
fprintDoubleWithPrefix (fout,ccunit,"%.2f ");
|
||||
fprintf (fout,"s resolution)\n------------\n");
|
||||
if (num < 1) return;
|
||||
|
||||
// get maximum description length
|
||||
maxl = 0;
|
||||
for (i=0; i<num; i++) {
|
||||
int l = strlen (event[i].description);
|
||||
if (l > maxl) maxl = l;
|
||||
}
|
||||
|
||||
// calculate total time
|
||||
double t1 = loadClockCount (event[0].cc);
|
||||
double t2 = loadClockCount (event[num-1].cc);
|
||||
double total = t2 - t1;
|
||||
if (total <= 0) total = 1;
|
||||
|
||||
// compute time difference for all slots except the last one. update totals
|
||||
double *times = (double*) ALLOCA (num * sizeof(double));
|
||||
for (i=0; i < (num-1); i++) {
|
||||
double t1 = loadClockCount (event[i].cc);
|
||||
double t2 = loadClockCount (event[i+1].cc);
|
||||
times[i] = t2 - t1;
|
||||
event[i].count++;
|
||||
event[i].total_t += times[i];
|
||||
event[i].total_p += times[i]/total * 100.0;
|
||||
}
|
||||
|
||||
// print report (with optional averages)
|
||||
for (i=0; i<num; i++) {
|
||||
double t,p;
|
||||
if (i < (num-1)) {
|
||||
t = times[i];
|
||||
p = t/total * 100.0;
|
||||
}
|
||||
else {
|
||||
t = total;
|
||||
p = 100.0;
|
||||
}
|
||||
fprintf (fout,"%-*s %7.2fms %6.2f%%",maxl,event[i].description,
|
||||
t*ccunit * 1000.0, p);
|
||||
if (average && i < (num-1)) {
|
||||
fprintf (fout," (avg %7.2fms %6.2f%%)",
|
||||
(event[i].total_t / event[i].count)*ccunit * 1000.0,
|
||||
event[i].total_p / event[i].count);
|
||||
}
|
||||
fprintf (fout,"\n");
|
||||
}
|
||||
fprintf (fout,"\n");
|
||||
}
|
||||
42
extern/ode/dist/tools/build4
vendored
42
extern/ode/dist/tools/build4
vendored
@@ -1,42 +0,0 @@
|
||||
#!/bin/sh
|
||||
#
|
||||
# build all four precision/release configurations and log the build messages
|
||||
# (used for debugging).
|
||||
|
||||
PLATFORM=unix-gcc
|
||||
SETTINGS=config/user-settings
|
||||
|
||||
if [ ! -f ode/src/ode.cpp ]; then
|
||||
echo "run this from the ODE root directory"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
function build() {
|
||||
echo -e "$PRECISION $MODE\n\n" >> BUILD_LOG
|
||||
cat <<END > $SETTINGS
|
||||
PLATFORM=$PLATFORM
|
||||
PRECISION=$PRECISION
|
||||
BUILD=$MODE
|
||||
END
|
||||
make clean
|
||||
make >> BUILD_LOG 2>&1
|
||||
echo -e "\n\n---------------------------------------------\n\n" >> BUILD_LOG
|
||||
}
|
||||
|
||||
echo > BUILD_LOG
|
||||
|
||||
PRECISION=SINGLE
|
||||
MODE=debug
|
||||
build
|
||||
PRECISION=SINGLE
|
||||
MODE=release
|
||||
build
|
||||
PRECISION=DOUBLE
|
||||
MODE=debug
|
||||
build
|
||||
PRECISION=DOUBLE
|
||||
MODE=release
|
||||
build
|
||||
|
||||
make clean
|
||||
rm -f $SETTINGS
|
||||
43
extern/ode/dist/tools/build4.bat
vendored
43
extern/ode/dist/tools/build4.bat
vendored
@@ -1,43 +0,0 @@
|
||||
@echo off
|
||||
rem build all four precision/release configurations and log the build messages
|
||||
rem (used for debugging).
|
||||
|
||||
setlocal
|
||||
|
||||
set PLATFORM=cygwin
|
||||
set SETTINGS=config\user-settings
|
||||
|
||||
echo SINGLE debug > BUILD_LOG
|
||||
echo PLATFORM=%PLATFORM%> %SETTINGS%
|
||||
echo PRECISION=SINGLE>> %SETTINGS%
|
||||
echo BUILD=debug>> %SETTINGS%
|
||||
make clean
|
||||
make >> BUILD_LOG
|
||||
echo --------------------------------------------- >> BUILD_LOG
|
||||
|
||||
echo DOUBLE debug >> BUILD_LOG
|
||||
echo PLATFORM=%PLATFORM%> %SETTINGS%
|
||||
echo PRECISION=DOUBLE>> %SETTINGS%
|
||||
echo BUILD=debug>> %SETTINGS%
|
||||
make clean
|
||||
make >> BUILD_LOG
|
||||
echo --------------------------------------------- >> BUILD_LOG
|
||||
|
||||
echo SINGLE release >> BUILD_LOG
|
||||
echo PLATFORM=%PLATFORM%> %SETTINGS%
|
||||
echo PRECISION=SINGLE>> %SETTINGS%
|
||||
echo BUILD=release>> %SETTINGS%
|
||||
make clean
|
||||
make >> BUILD_LOG
|
||||
echo --------------------------------------------- >> BUILD_LOG
|
||||
|
||||
echo DOUBLE release >> BUILD_LOG
|
||||
echo PLATFORM=%PLATFORM%> %SETTINGS%
|
||||
echo PRECISION=DOUBLE>> %SETTINGS%
|
||||
echo BUILD=release>> %SETTINGS%
|
||||
make clean
|
||||
make >> BUILD_LOG
|
||||
echo --------------------------------------------- >> BUILD_LOG
|
||||
|
||||
make clean
|
||||
del %SETTINGS%
|
||||
45
extern/ode/dist/tools/make_distribution
vendored
45
extern/ode/dist/tools/make_distribution
vendored
@@ -1,45 +0,0 @@
|
||||
#!/bin/sh
|
||||
|
||||
VER=0.03
|
||||
# VER=`date +%y%m%d`
|
||||
|
||||
if [ ! -f ode/src/ode.cpp ]; then
|
||||
echo "run this from the ODE root directory"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
ODE_DIR=`pwd`
|
||||
|
||||
cd /tmp
|
||||
if [ -d /tmp/ode-$VER ]; then
|
||||
echo "remove /tmp/ode-$VER first"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
mkdir /tmp/ode-$VER
|
||||
cp -av $ODE_DIR/* /tmp/ode-$VER
|
||||
find /tmp/ode-$VER -type d -name CVS -exec rm -rf {} \; -print
|
||||
find /tmp/ode-$VER -type f -name *~ -exec rm -f {} \; -print
|
||||
rmdir /tmp/ode-$VER/build
|
||||
|
||||
cd /tmp/ode-$VER
|
||||
make clean
|
||||
cp config/user-settings.example config/user-settings
|
||||
|
||||
cd ode/doc
|
||||
./doccer ode.doc > ode.html
|
||||
|
||||
cd /tmp/ode-$VER
|
||||
echo -e "\n\nMake any modifications you want, then exit the shell:"
|
||||
bash
|
||||
|
||||
cd /tmp
|
||||
tar cfvz ode-$VER.tgz ode-$VER
|
||||
rm -rf /tmp/ode-$VER
|
||||
|
||||
echo -e "\ntype <return> to exit or 'c' to copy to q12"
|
||||
read Q
|
||||
if [ $Q ]; then
|
||||
echo copying...
|
||||
scp1 ode-$VER.tgz q12.org:~/q12/ode/release/
|
||||
fi
|
||||
11
extern/ode/dist/tools/process_deps
vendored
11
extern/ode/dist/tools/process_deps
vendored
@@ -1,11 +0,0 @@
|
||||
#!/usr/bin/perl
|
||||
|
||||
$a = join ('',<STDIN>);
|
||||
$a =~ s/\\\n/ /g; # join continued lines
|
||||
$a =~ s/(^\S+:)/$ARGV[0]$1/gm; # put prefix in front of rules
|
||||
$a =~ s/\s+\/\S+/ /g; # remove absolute path dependencies
|
||||
$a =~ s/\s+\n/\n/g; # remove whitespace at end of lines
|
||||
$a =~ s/[ \t]+/ /g; # clean up interior whitespace
|
||||
$a =~ s/ / \\\n /g; # put back line continuations
|
||||
|
||||
print $a;
|
||||
22
extern/ode/patchfile.FreeBSD
vendored
22
extern/ode/patchfile.FreeBSD
vendored
@@ -1,22 +0,0 @@
|
||||
--- dist/Makefile.org Sat Jan 11 23:55:46 2003
|
||||
+++ dist/Makefile Sat Jan 11 23:55:36 2003
|
||||
@@ -242,14 +242,16 @@
|
||||
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)
|
||||
|
||||
+# Patched for FreeBSD
|
||||
+
|
||||
%$(OBJ): %.c
|
||||
- $(CC) $(C_FLAGS) $(C_INC)$(INCPATH) $(DEFINES) $(C_OPT)1 $(C_OUT)$@ $<
|
||||
+ $(CC) $(C_FLAGS) $(C_INC)$(INCPATH) -I/usr/X11R6/include $(DEFINES) $(C_OPT)1 $(C_OUT)$@ $<
|
||||
|
||||
%$(OBJ): %.cpp
|
||||
- $(CC) $(C_FLAGS) $(C_INC)$(INCPATH) $(DEFINES) $(C_OPT)$(OPT) $(C_OUT)$@ $<
|
||||
+ $(CC) $(C_FLAGS) $(C_INC)$(INCPATH) -I/usr/X11R6/include $(DEFINES) $(C_OPT)$(OPT) $(C_OUT)$@ $<
|
||||
|
||||
%.exe: %$(OBJ)
|
||||
- $(CC) $(C_EXEOUT)$@ $< $(ODE_LIB) $(DRAWSTUFF_LIB) $(RESOURCE_FILE) $(LINK_OPENGL) $(LINK_MATH)
|
||||
+ $(CC) $(C_EXEOUT)$@ $< $(ODE_LIB) $(DRAWSTUFF_LIB) $(RESOURCE_FILE) $(LINK_OPENGL) $(LINK_MATH) -lXext
|
||||
|
||||
# windows specific rules
|
||||
|
||||
Reference in New Issue
Block a user