rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

OP-191: Merge from full-calibration branch. Import Eigen 2.0.15 to the GCS.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@3051 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
jonathan 2011-03-21 00:43:09 +00:00 committed by jonathan
parent f2a7f8e187
commit bf191fed27
130 changed files with 30146 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

674
ground/openpilotgcs/src/libs/eigen/COPYING Normal file
View File

@ -0,0 +1,674 @@
GNU GENERAL PUBLIC LICENSE
Version 3, 29 June 2007
Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
Preamble
The GNU General Public License is a free, copyleft license for
software and other kinds of works.
The licenses for most software and other practical works are designed
to take away your freedom to share and change the works. By contrast,
the GNU General Public License is intended to guarantee your freedom to
share and change all versions of a program--to make sure it remains free
software for all its users. We, the Free Software Foundation, use the
GNU General Public License for most of our software; it applies also to
any other work released this way by its authors. You can apply it to
your programs, too.
When we speak of free software, we are referring to freedom, 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
them if you wish), that you receive source code or can get it if you
want it, that you can change the software or use pieces of it in new
free programs, and that you know you can do these things.
To protect your rights, we need to prevent others from denying you
these rights or asking you to surrender the rights. Therefore, you have
certain responsibilities if you distribute copies of the software, or if
you modify it: responsibilities to respect the freedom of others.
For example, if you distribute copies of such a program, whether
gratis or for a fee, you must pass on to the recipients the same
freedoms that you received. You must make sure that they, too, receive
or can get the source code. And you must show them these terms so they
know their rights.
Developers that use the GNU GPL protect your rights with two steps:
(1) assert copyright on the software, and (2) offer you this License
giving you legal permission to copy, distribute and/or modify it.
For the developers' and authors' protection, the GPL clearly explains
that there is no warranty for this free software. For both users' and
authors' sake, the GPL requires that modified versions be marked as
changed, so that their problems will not be attributed erroneously to
authors of previous versions.
Some devices are designed to deny users access to install or run
modified versions of the software inside them, although the manufacturer
can do so. This is fundamentally incompatible with the aim of
protecting users' freedom to change the software. The systematic
pattern of such abuse occurs in the area of products for individuals to
use, which is precisely where it is most unacceptable. Therefore, we
have designed this version of the GPL to prohibit the practice for those
products. If such problems arise substantially in other domains, we
stand ready to extend this provision to those domains in future versions
of the GPL, as needed to protect the freedom of users.
Finally, every program is threatened constantly by software patents.
States should not allow patents to restrict development and use of
software on general-purpose computers, but in those that do, we wish to
avoid the special danger that patents applied to a free program could
make it effectively proprietary. To prevent this, the GPL assures that
patents cannot be used to render the program non-free.
The precise terms and conditions for copying, distribution and
modification follow.
TERMS AND CONDITIONS
0. Definitions.
"This License" refers to version 3 of the GNU General Public License.
"Copyright" also means copyright-like laws that apply to other kinds of
works, such as semiconductor masks.
"The Program" refers to any copyrightable work licensed under this
License. Each licensee is addressed as "you". "Licensees" and
"recipients" may be individuals or organizations.
To "modify" a work means to copy from or adapt all or part of the work
in a fashion requiring copyright permission, other than the making of an
exact copy. The resulting work is called a "modified version" of the
earlier work or a work "based on" the earlier work.
A "covered work" means either the unmodified Program or a work based
on the Program.
To "propagate" a work means to do anything with it that, without
permission, would make you directly or secondarily liable for
infringement under applicable copyright law, except executing it on a
computer or modifying a private copy. Propagation includes copying,
distribution (with or without modification), making available to the
public, and in some countries other activities as well.
To "convey" a work means any kind of propagation that enables other
parties to make or receive copies. Mere interaction with a user through
a computer network, with no transfer of a copy, is not conveying.
An interactive user interface displays "Appropriate Legal Notices"
to the extent that it includes a convenient and prominently visible
feature that (1) displays an appropriate copyright notice, and (2)
tells the user that there is no warranty for the work (except to the
extent that warranties are provided), that licensees may convey the
work under this License, and how to view a copy of this License. If
the interface presents a list of user commands or options, such as a
menu, a prominent item in the list meets this criterion.
1. Source Code.
The "source code" for a work means the preferred form of the work
for making modifications to it. "Object code" means any non-source
form of a work.
A "Standard Interface" means an interface that either is an official
standard defined by a recognized standards body, or, in the case of
interfaces specified for a particular programming language, one that
is widely used among developers working in that language.
The "System Libraries" of an executable work include anything, other
than the work as a whole, that (a) is included in the normal form of
packaging a Major Component, but which is not part of that Major
Component, and (b) serves only to enable use of the work with that
Major Component, or to implement a Standard Interface for which an
implementation is available to the public in source code form. A
"Major Component", in this context, means a major essential component
(kernel, window system, and so on) of the specific operating system
(if any) on which the executable work runs, or a compiler used to
produce the work, or an object code interpreter used to run it.
The "Corresponding Source" for a work in object code form means all
the source code needed to generate, install, and (for an executable
work) run the object code and to modify the work, including scripts to
control those activities. However, it does not include the work's
System Libraries, or general-purpose tools or generally available free
programs which are used unmodified in performing those activities but
which are not part of the work. For example, Corresponding Source
includes interface definition files associated with source files for
the work, and the source code for shared libraries and dynamically
linked subprograms that the work is specifically designed to require,
such as by intimate data communication or control flow between those
subprograms and other parts of the work.
The Corresponding Source need not include anything that users
can regenerate automatically from other parts of the Corresponding
Source.
The Corresponding Source for a work in source code form is that
same work.
2. Basic Permissions.
All rights granted under this License are granted for the term of
copyright on the Program, and are irrevocable provided the stated
conditions are met. This License explicitly affirms your unlimited
permission to run the unmodified Program. The output from running a
covered work is covered by this License only if the output, given its
content, constitutes a covered work. This License acknowledges your
rights of fair use or other equivalent, as provided by copyright law.
You may make, run and propagate covered works that you do not
convey, without conditions so long as your license otherwise remains
in force. You may convey covered works to others for the sole purpose
of having them make modifications exclusively for you, or provide you
with facilities for running those works, provided that you comply with
the terms of this License in conveying all material for which you do
not control copyright. Those thus making or running the covered works
for you must do so exclusively on your behalf, under your direction
and control, on terms that prohibit them from making any copies of
your copyrighted material outside their relationship with you.
Conveying under any other circumstances is permitted solely under
the conditions stated below. Sublicensing is not allowed; section 10
makes it unnecessary.
3. Protecting Users' Legal Rights From Anti-Circumvention Law.
No covered work shall be deemed part of an effective technological
measure under any applicable law fulfilling obligations under article
11 of the WIPO copyright treaty adopted on 20 December 1996, or
similar laws prohibiting or restricting circumvention of such
measures.
When you convey a covered work, you waive any legal power to forbid
circumvention of technological measures to the extent such circumvention
is effected by exercising rights under this License with respect to
the covered work, and you disclaim any intention to limit operation or
modification of the work as a means of enforcing, against the work's
users, your or third parties' legal rights to forbid circumvention of
technological measures.
4. Conveying Verbatim Copies.
You may convey verbatim copies of the Program's source code as you
receive it, in any medium, provided that you conspicuously and
appropriately publish on each copy an appropriate copyright notice;
keep intact all notices stating that this License and any
non-permissive terms added in accord with section 7 apply to the code;
keep intact all notices of the absence of any warranty; and give all
recipients a copy of this License along with the Program.
You may charge any price or no price for each copy that you convey,
and you may offer support or warranty protection for a fee.
5. Conveying Modified Source Versions.
You may convey a work based on the Program, or the modifications to
produce it from the Program, in the form of source code under the
terms of section 4, provided that you also meet all of these conditions:
a) The work must carry prominent notices stating that you modified
it, and giving a relevant date.
b) The work must carry prominent notices stating that it is
released under this License and any conditions added under section
7. This requirement modifies the requirement in section 4 to
"keep intact all notices".
c) You must license the entire work, as a whole, under this
License to anyone who comes into possession of a copy. This
License will therefore apply, along with any applicable section 7
additional terms, to the whole of the work, and all its parts,
regardless of how they are packaged. This License gives no
permission to license the work in any other way, but it does not
invalidate such permission if you have separately received it.
d) If the work has interactive user interfaces, each must display
Appropriate Legal Notices; however, if the Program has interactive
interfaces that do not display Appropriate Legal Notices, your
work need not make them do so.
A compilation of a covered work with other separate and independent
works, which are not by their nature extensions of the covered work,
and which are not combined with it such as to form a larger program,
in or on a volume of a storage or distribution medium, is called an
"aggregate" if the compilation and its resulting copyright are not
used to limit the access or legal rights of the compilation's users
beyond what the individual works permit. Inclusion of a covered work
in an aggregate does not cause this License to apply to the other
parts of the aggregate.
6. Conveying Non-Source Forms.
You may convey a covered work in object code form under the terms
of sections 4 and 5, provided that you also convey the
machine-readable Corresponding Source under the terms of this License,
in one of these ways:
a) Convey the object code in, or embodied in, a physical product
(including a physical distribution medium), accompanied by the
Corresponding Source fixed on a durable physical medium
customarily used for software interchange.
b) Convey the object code in, or embodied in, a physical product
(including a physical distribution medium), accompanied by a
written offer, valid for at least three years and valid for as
long as you offer spare parts or customer support for that product
model, to give anyone who possesses the object code either (1) a
copy of the Corresponding Source for all the software in the
product that is covered by this License, on a durable physical
medium customarily used for software interchange, for a price no
more than your reasonable cost of physically performing this
conveying of source, or (2) access to copy the
Corresponding Source from a network server at no charge.
c) Convey individual copies of the object code with a copy of the
written offer to provide the Corresponding Source. This
alternative is allowed only occasionally and noncommercially, and
only if you received the object code with such an offer, in accord
with subsection 6b.
d) Convey the object code by offering access from a designated
place (gratis or for a charge), and offer equivalent access to the
Corresponding Source in the same way through the same place at no
further charge. You need not require recipients to copy the
Corresponding Source along with the object code. If the place to
copy the object code is a network server, the Corresponding Source
may be on a different server (operated by you or a third party)
that supports equivalent copying facilities, provided you maintain
clear directions next to the object code saying where to find the
Corresponding Source. Regardless of what server hosts the
Corresponding Source, you remain obligated to ensure that it is
available for as long as needed to satisfy these requirements.
e) Convey the object code using peer-to-peer transmission, provided
you inform other peers where the object code and Corresponding
Source of the work are being offered to the general public at no
charge under subsection 6d.
A separable portion of the object code, whose source code is excluded
from the Corresponding Source as a System Library, need not be
included in conveying the object code work.
A "User Product" is either (1) a "consumer product", which means any
tangible personal property which is normally used for personal, family,
or household purposes, or (2) anything designed or sold for incorporation
into a dwelling. In determining whether a product is a consumer product,
doubtful cases shall be resolved in favor of coverage. For a particular
product received by a particular user, "normally used" refers to a
typical or common use of that class of product, regardless of the status
of the particular user or of the way in which the particular user
actually uses, or expects or is expected to use, the product. A product
is a consumer product regardless of whether the product has substantial
commercial, industrial or non-consumer uses, unless such uses represent
the only significant mode of use of the product.
"Installation Information" for a User Product means any methods,
procedures, authorization keys, or other information required to install
and execute modified versions of a covered work in that User Product from
a modified version of its Corresponding Source. The information must
suffice to ensure that the continued functioning of the modified object
code is in no case prevented or interfered with solely because
modification has been made.
If you convey an object code work under this section in, or with, or
specifically for use in, a User Product, and the conveying occurs as
part of a transaction in which the right of possession and use of the
User Product is transferred to the recipient in perpetuity or for a
fixed term (regardless of how the transaction is characterized), the
Corresponding Source conveyed under this section must be accompanied
by the Installation Information. But this requirement does not apply
if neither you nor any third party retains the ability to install
modified object code on the User Product (for example, the work has
been installed in ROM).
The requirement to provide Installation Information does not include a
requirement to continue to provide support service, warranty, or updates
for a work that has been modified or installed by the recipient, or for
the User Product in which it has been modified or installed. Access to a
network may be denied when the modification itself materially and
adversely affects the operation of the network or violates the rules and
protocols for communication across the network.
Corresponding Source conveyed, and Installation Information provided,
in accord with this section must be in a format that is publicly
documented (and with an implementation available to the public in
source code form), and must require no special password or key for
unpacking, reading or copying.
7. Additional Terms.
"Additional permissions" are terms that supplement the terms of this
License by making exceptions from one or more of its conditions.
Additional permissions that are applicable to the entire Program shall
be treated as though they were included in this License, to the extent
that they are valid under applicable law. If additional permissions
apply only to part of the Program, that part may be used separately
under those permissions, but the entire Program remains governed by
this License without regard to the additional permissions.
When you convey a copy of a covered work, you may at your option
remove any additional permissions from that copy, or from any part of
it. (Additional permissions may be written to require their own
removal in certain cases when you modify the work.) You may place
additional permissions on material, added by you to a covered work,
for which you have or can give appropriate copyright permission.
Notwithstanding any other provision of this License, for material you
add to a covered work, you may (if authorized by the copyright holders of
that material) supplement the terms of this License with terms:
a) Disclaiming warranty or limiting liability differently from the
terms of sections 15 and 16 of this License; or
b) Requiring preservation of specified reasonable legal notices or
author attributions in that material or in the Appropriate Legal
Notices displayed by works containing it; or
c) Prohibiting misrepresentation of the origin of that material, or
requiring that modified versions of such material be marked in
reasonable ways as different from the original version; or
d) Limiting the use for publicity purposes of names of licensors or
authors of the material; or
e) Declining to grant rights under trademark law for use of some
trade names, trademarks, or service marks; or
f) Requiring indemnification of licensors and authors of that
material by anyone who conveys the material (or modified versions of
it) with contractual assumptions of liability to the recipient, for
any liability that these contractual assumptions directly impose on
those licensors and authors.
All other non-permissive additional terms are considered "further
restrictions" within the meaning of section 10. If the Program as you
received it, or any part of it, contains a notice stating that it is
governed by this License along with a term that is a further
restriction, you may remove that term. If a license document contains
a further restriction but permits relicensing or conveying under this
License, you may add to a covered work material governed by the terms
of that license document, provided that the further restriction does
not survive such relicensing or conveying.
If you add terms to a covered work in accord with this section, you
must place, in the relevant source files, a statement of the
additional terms that apply to those files, or a notice indicating
where to find the applicable terms.
Additional terms, permissive or non-permissive, may be stated in the
form of a separately written license, or stated as exceptions;
the above requirements apply either way.
8. Termination.
You may not propagate or modify a covered work except as expressly
provided under this License. Any attempt otherwise to propagate or
modify it is void, and will automatically terminate your rights under
this License (including any patent licenses granted under the third
paragraph of section 11).
However, if you cease all violation of this License, then your
license from a particular copyright holder is reinstated (a)
provisionally, unless and until the copyright holder explicitly and
finally terminates your license, and (b) permanently, if the copyright
holder fails to notify you of the violation by some reasonable means
prior to 60 days after the cessation.
Moreover, your license from a particular copyright holder is
reinstated permanently if the copyright holder notifies you of the
violation by some reasonable means, this is the first time you have
received notice of violation of this License (for any work) from that
copyright holder, and you cure the violation prior to 30 days after
your receipt of the notice.
Termination of your rights under this section does not terminate the
licenses of parties who have received copies or rights from you under
this License. If your rights have been terminated and not permanently
reinstated, you do not qualify to receive new licenses for the same
material under section 10.
9. Acceptance Not Required for Having Copies.
You are not required to accept this License in order to receive or
run a copy of the Program. Ancillary propagation of a covered work
occurring solely as a consequence of using peer-to-peer transmission
to receive a copy likewise does not require acceptance. However,
nothing other than this License grants you permission to propagate or
modify any covered work. These actions infringe copyright if you do
not accept this License. Therefore, by modifying or propagating a
covered work, you indicate your acceptance of this License to do so.
10. Automatic Licensing of Downstream Recipients.
Each time you convey a covered work, the recipient automatically
receives a license from the original licensors, to run, modify and
propagate that work, subject to this License. You are not responsible
for enforcing compliance by third parties with this License.
An "entity transaction" is a transaction transferring control of an
organization, or substantially all assets of one, or subdividing an
organization, or merging organizations. If propagation of a covered
work results from an entity transaction, each party to that
transaction who receives a copy of the work also receives whatever
licenses to the work the party's predecessor in interest had or could
give under the previous paragraph, plus a right to possession of the
Corresponding Source of the work from the predecessor in interest, if
the predecessor has it or can get it with reasonable efforts.
You may not impose any further restrictions on the exercise of the
rights granted or affirmed under this License. For example, you may
not impose a license fee, royalty, or other charge for exercise of
rights granted under this License, and you may not initiate litigation
(including a cross-claim or counterclaim in a lawsuit) alleging that
any patent claim is infringed by making, using, selling, offering for
sale, or importing the Program or any portion of it.
11. Patents.
A "contributor" is a copyright holder who authorizes use under this
License of the Program or a work on which the Program is based. The
work thus licensed is called the contributor's "contributor version".
A contributor's "essential patent claims" are all patent claims
owned or controlled by the contributor, whether already acquired or
hereafter acquired, that would be infringed by some manner, permitted
by this License, of making, using, or selling its contributor version,
but do not include claims that would be infringed only as a
consequence of further modification of the contributor version. For
purposes of this definition, "control" includes the right to grant
patent sublicenses in a manner consistent with the requirements of
this License.
Each contributor grants you a non-exclusive, worldwide, royalty-free
patent license under the contributor's essential patent claims, to
make, use, sell, offer for sale, import and otherwise run, modify and
propagate the contents of its contributor version.
In the following three paragraphs, a "patent license" is any express
agreement or commitment, however denominated, not to enforce a patent
(such as an express permission to practice a patent or covenant not to
sue for patent infringement). To "grant" such a patent license to a
party means to make such an agreement or commitment not to enforce a
patent against the party.
If you convey a covered work, knowingly relying on a patent license,
and the Corresponding Source of the work is not available for anyone
to copy, free of charge and under the terms of this License, through a
publicly available network server or other readily accessible means,
then you must either (1) cause the Corresponding Source to be so
available, or (2) arrange to deprive yourself of the benefit of the
patent license for this particular work, or (3) arrange, in a manner
consistent with the requirements of this License, to extend the patent
license to downstream recipients. "Knowingly relying" means you have
actual knowledge that, but for the patent license, your conveying the
covered work in a country, or your recipient's use of the covered work
in a country, would infringe one or more identifiable patents in that
country that you have reason to believe are valid.
If, pursuant to or in connection with a single transaction or
arrangement, you convey, or propagate by procuring conveyance of, a
covered work, and grant a patent license to some of the parties
receiving the covered work authorizing them to use, propagate, modify
or convey a specific copy of the covered work, then the patent license
you grant is automatically extended to all recipients of the covered
work and works based on it.
A patent license is "discriminatory" if it does not include within
the scope of its coverage, prohibits the exercise of, or is
conditioned on the non-exercise of one or more of the rights that are
specifically granted under this License. You may not convey a covered
work if you are a party to an arrangement with a third party that is
in the business of distributing software, under which you make payment
to the third party based on the extent of your activity of conveying
the work, and under which the third party grants, to any of the
parties who would receive the covered work from you, a discriminatory
patent license (a) in connection with copies of the covered work
conveyed by you (or copies made from those copies), or (b) primarily
for and in connection with specific products or compilations that
contain the covered work, unless you entered into that arrangement,
or that patent license was granted, prior to 28 March 2007.
Nothing in this License shall be construed as excluding or limiting
any implied license or other defenses to infringement that may
otherwise be available to you under applicable patent law.
12. No Surrender of Others' Freedom.
If 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 convey a
covered work so as to satisfy simultaneously your obligations under this
License and any other pertinent obligations, then as a consequence you may
not convey it at all. For example, if you agree to terms that obligate you
to collect a royalty for further conveying from those to whom you convey
the Program, the only way you could satisfy both those terms and this
License would be to refrain entirely from conveying the Program.
13. Use with the GNU Affero General Public License.
Notwithstanding any other provision of this License, you have
permission to link or combine any covered work with a work licensed
under version 3 of the GNU Affero General Public License into a single
combined work, and to convey the resulting work. The terms of this
License will continue to apply to the part which is the covered work,
but the special requirements of the GNU Affero General Public License,
section 13, concerning interaction through a network will apply to the
combination as such.
14. Revised Versions of this License.
The Free Software Foundation may publish revised and/or new versions of
the GNU 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
Program specifies that a certain numbered version of the GNU General
Public License "or any later version" applies to it, you have the
option of following the terms and conditions either of that numbered
version or of any later version published by the Free Software
Foundation. If the Program does not specify a version number of the
GNU General Public License, you may choose any version ever published
by the Free Software Foundation.
If the Program specifies that a proxy can decide which future
versions of the GNU General Public License can be used, that proxy's
public statement of acceptance of a version permanently authorizes you
to choose that version for the Program.
Later license versions may give you additional or different
permissions. However, no additional obligations are imposed on any
author or copyright holder as a result of your choosing to follow a
later version.
15. Disclaimer of Warranty.
THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "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 PROGRAM
IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
16. Limitation of Liability.
IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
THE PROGRAM 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 PROGRAM (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 PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
SUCH DAMAGES.
17. Interpretation of Sections 15 and 16.
If the disclaimer of warranty and limitation of liability provided
above cannot be given local legal effect according to their terms,
reviewing courts shall apply local law that most closely approximates
an absolute waiver of all civil liability in connection with the
Program, unless a warranty or assumption of liability accompanies a
copy of the Program in return for a fee.
END OF TERMS AND CONDITIONS
How to Apply These Terms to Your New Programs
If you develop a new program, and you want it to be of the greatest
possible use to the public, the best way to achieve this is to make it
free software which everyone can redistribute and change under these terms.
To do so, attach the following notices to the program. It is safest
to attach them to the start of each source file to most effectively
state 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 program's name and a brief idea of what it does.>
Copyright (C) <year> <name of author>
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 3 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, see <http://www.gnu.org/licenses/>.
Also add information on how to contact you by electronic and paper mail.
If the program does terminal interaction, make it output a short
notice like this when it starts in an interactive mode:
<program> Copyright (C) <year> <name of author>
This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
This is free software, and you are welcome to redistribute it
under certain conditions; type `show c' for details.
The hypothetical commands `show w' and `show c' should show the appropriate
parts of the General Public License. Of course, your program's commands
might be different; for a GUI interface, you would use an "about box".
You should also get your employer (if you work as a programmer) or school,
if any, to sign a "copyright disclaimer" for the program, if necessary.
For more information on this, and how to apply and follow the GNU GPL, see
<http://www.gnu.org/licenses/>.
The GNU General Public License does not permit incorporating your program
into proprietary programs. If your program is a subroutine library, you
may consider it more useful to permit linking proprietary applications with
the library. If this is what you want to do, use the GNU Lesser General
Public License instead of this License. But first, please read
<http://www.gnu.org/philosophy/why-not-lgpl.html>.

165
ground/openpilotgcs/src/libs/eigen/COPYING.LESSER Normal file
View File

@ -0,0 +1,165 @@
GNU LESSER GENERAL PUBLIC LICENSE
Version 3, 29 June 2007
Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
This version of the GNU Lesser General Public License incorporates
the terms and conditions of version 3 of the GNU General Public
License, supplemented by the additional permissions listed below.
0. Additional Definitions.
As used herein, "this License" refers to version 3 of the GNU Lesser
General Public License, and the "GNU GPL" refers to version 3 of the GNU
General Public License.
"The Library" refers to a covered work governed by this License,
other than an Application or a Combined Work as defined below.
An "Application" is any work that makes use of an interface provided
by the Library, but which is not otherwise based on the Library.
Defining a subclass of a class defined by the Library is deemed a mode
of using an interface provided by the Library.
A "Combined Work" is a work produced by combining or linking an
Application with the Library. The particular version of the Library
with which the Combined Work was made is also called the "Linked
Version".
The "Minimal Corresponding Source" for a Combined Work means the
Corresponding Source for the Combined Work, excluding any source code
for portions of the Combined Work that, considered in isolation, are
based on the Application, and not on the Linked Version.
The "Corresponding Application Code" for a Combined Work means the
object code and/or source code for the Application, including any data
and utility programs needed for reproducing the Combined Work from the
Application, but excluding the System Libraries of the Combined Work.
1. Exception to Section 3 of the GNU GPL.
You may convey a covered work under sections 3 and 4 of this License
without being bound by section 3 of the GNU GPL.
2. Conveying Modified Versions.
If you modify a copy of the Library, and, in your modifications, a
facility refers to a function or data to be supplied by an Application
that uses the facility (other than as an argument passed when the
facility is invoked), then you may convey a copy of the modified
version:
a) under this License, provided that you make a good faith effort to
ensure that, in the event an Application does not supply the
function or data, the facility still operates, and performs
whatever part of its purpose remains meaningful, or
b) under the GNU GPL, with none of the additional permissions of
this License applicable to that copy.
3. Object Code Incorporating Material from Library Header Files.
The object code form of an Application may incorporate material from
a header file that is part of the Library. You may convey such object
code under terms of your choice, provided that, if the incorporated
material is not limited to numerical parameters, data structure
layouts and accessors, or small macros, inline functions and templates
(ten or fewer lines in length), you do both of the following:
a) Give prominent notice with each copy of the object code that the
Library is used in it and that the Library and its use are
covered by this License.
b) Accompany the object code with a copy of the GNU GPL and this license
document.
4. Combined Works.
You may convey a Combined Work under terms of your choice that,
taken together, effectively do not restrict modification of the
portions of the Library contained in the Combined Work and reverse
engineering for debugging such modifications, if you also do each of
the following:
a) Give prominent notice with each copy of the Combined Work that
the Library is used in it and that the Library and its use are
covered by this License.
b) Accompany the Combined Work with a copy of the GNU GPL and this license
document.
c) For a Combined Work that displays copyright notices during
execution, include the copyright notice for the Library among
these notices, as well as a reference directing the user to the
copies of the GNU GPL and this license document.
d) Do one of the following:
0) Convey the Minimal Corresponding Source under the terms of this
License, and the Corresponding Application Code in a form
suitable for, and under terms that permit, the user to
recombine or relink the Application with a modified version of
the Linked Version to produce a modified Combined Work, in the
manner specified by section 6 of the GNU GPL for conveying
Corresponding Source.
1) Use a suitable shared library mechanism for linking with the
Library. A suitable mechanism is one that (a) uses at run time
a copy of the Library already present on the user's computer
system, and (b) will operate properly with a modified version
of the Library that is interface-compatible with the Linked
Version.
e) Provide Installation Information, but only if you would otherwise
be required to provide such information under section 6 of the
GNU GPL, and only to the extent that such information is
necessary to install and execute a modified version of the
Combined Work produced by recombining or relinking the
Application with a modified version of the Linked Version. (If
you use option 4d0, the Installation Information must accompany
the Minimal Corresponding Source and Corresponding Application
Code. If you use option 4d1, you must provide the Installation
Information in the manner specified by section 6 of the GNU GPL
for conveying Corresponding Source.)
5. Combined Libraries.
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 that are not Applications and are not covered by this
License, and convey such a combined library under terms of your
choice, if you do both of the following:
a) Accompany the combined library with a copy of the same work based
on the Library, uncombined with any other library facilities,
conveyed under the terms of this License.
b) Give prominent notice with the combined library that part of it
is a work based on the Library, and explaining where to find the
accompanying uncombined form of the same work.
6. Revised Versions of the GNU Lesser General Public License.
The Free Software Foundation may publish revised and/or new versions
of the GNU 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 as you received it specifies that a certain numbered version
of the GNU Lesser General Public License "or any later version"
applies to it, you have the option of following the terms and
conditions either of that published version or of any later version
published by the Free Software Foundation. If the Library as you
received it does not specify a version number of the GNU Lesser
General Public License, you may choose any version of the GNU Lesser
General Public License ever published by the Free Software Foundation.
If the Library as you received it specifies that a proxy can decide
whether future versions of the GNU Lesser General Public License shall
apply, that proxy's public statement of acceptance of any version is
permanent authorization for you to choose that version for the
Library.

39
ground/openpilotgcs/src/libs/eigen/Eigen/Array Normal file
View File

@ -0,0 +1,39 @@
#ifndef EIGEN_ARRAY_MODULE_H
#define EIGEN_ARRAY_MODULE_H
#include "Core"
#include "src/Core/util/DisableMSVCWarnings.h"
namespace Eigen {
/** \defgroup Array_Module Array module
* This module provides several handy features to manipulate matrices as simple array of values.
* In addition to listed classes, it defines various methods of the Cwise interface
* (accessible from MatrixBase::cwise()), including:
* - matrix-scalar sum,
* - coeff-wise comparison operators,
* - sin, cos, sqrt, pow, exp, log, square, cube, inverse (reciprocal).
*
* This module also provides various MatrixBase methods, including:
* - \ref MatrixBase::all() "all", \ref MatrixBase::any() "any",
* - \ref MatrixBase::Random() "random matrix initialization"
*
* \code
* #include <Eigen/Array>
* \endcode
*/
#include "src/Array/CwiseOperators.h"
#include "src/Array/Functors.h"
#include "src/Array/BooleanRedux.h"
#include "src/Array/Select.h"
#include "src/Array/PartialRedux.h"
#include "src/Array/Random.h"
#include "src/Array/Norms.h"
} // namespace Eigen
#include "src/Core/util/EnableMSVCWarnings.h"
#endif // EIGEN_ARRAY_MODULE_H

31
ground/openpilotgcs/src/libs/eigen/Eigen/CMakeLists.txt Normal file
View File

@ -0,0 +1,31 @@
set(Eigen_HEADERS Core LU Cholesky QR Geometry
Sparse Array SVD LeastSquares
QtAlignedMalloc StdVector NewStdVector
Eigen Dense)
if(EIGEN_BUILD_LIB)
set(Eigen_SRCS
src/Core/CoreInstantiations.cpp
src/Cholesky/CholeskyInstantiations.cpp
src/QR/QrInstantiations.cpp
)
add_library(Eigen2 SHARED ${Eigen_SRCS})
install(TARGETS Eigen2
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib)
endif(EIGEN_BUILD_LIB)
if(CMAKE_COMPILER_IS_GNUCXX)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g1 -O2")
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} -g1 -O2")
endif(CMAKE_COMPILER_IS_GNUCXX)
install(FILES
${Eigen_HEADERS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen
)
add_subdirectory(src)

65
ground/openpilotgcs/src/libs/eigen/Eigen/Cholesky Normal file
View File

@ -0,0 +1,65 @@
#ifndef EIGEN_CHOLESKY_MODULE_H
#define EIGEN_CHOLESKY_MODULE_H
#include "Core"
#include "src/Core/util/DisableMSVCWarnings.h"
// Note that EIGEN_HIDE_HEAVY_CODE has to be defined per module
#if (defined EIGEN_EXTERN_INSTANTIATIONS) && (EIGEN_EXTERN_INSTANTIATIONS>=2)
#ifndef EIGEN_HIDE_HEAVY_CODE
#define EIGEN_HIDE_HEAVY_CODE
#endif
#elif defined EIGEN_HIDE_HEAVY_CODE
#undef EIGEN_HIDE_HEAVY_CODE
#endif
namespace Eigen {
/** \defgroup Cholesky_Module Cholesky module
*
* \nonstableyet
*
* This module provides two variants of the Cholesky decomposition for selfadjoint (hermitian) matrices.
* Those decompositions are accessible via the following MatrixBase methods:
* - MatrixBase::llt(),
* - MatrixBase::ldlt()
*
* \code
* #include <Eigen/Cholesky>
* \endcode
*/
#include "src/Array/CwiseOperators.h"
#include "src/Array/Functors.h"
#include "src/Cholesky/LLT.h"
#include "src/Cholesky/LDLT.h"
} // namespace Eigen
#define EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(MATRIXTYPE,PREFIX) \
PREFIX template class LLT<MATRIXTYPE>; \
PREFIX template class LDLT<MATRIXTYPE>
#define EIGEN_CHOLESKY_MODULE_INSTANTIATE(PREFIX) \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix2f,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix2d,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix3f,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix3d,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix4f,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(Matrix4d,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(MatrixXf,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(MatrixXd,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(MatrixXcf,PREFIX); \
EIGEN_CHOLESKY_MODULE_INSTANTIATE_TYPE(MatrixXcd,PREFIX)
#ifdef EIGEN_EXTERN_INSTANTIATIONS
namespace Eigen {
EIGEN_CHOLESKY_MODULE_INSTANTIATE(extern);
} // namespace Eigen
#endif
#include "src/Core/util/EnableMSVCWarnings.h"
#endif // EIGEN_CHOLESKY_MODULE_H

160
ground/openpilotgcs/src/libs/eigen/Eigen/Core Normal file
View File

@ -0,0 +1,160 @@
#ifndef EIGEN_CORE_H
#define EIGEN_CORE_H
// first thing Eigen does: prevent MSVC from committing suicide
#include "src/Core/util/DisableMSVCWarnings.h"
#ifdef _MSC_VER
#include <malloc.h> // for _aligned_malloc -- need it regardless of whether vectorization is enabled
#if (_MSC_VER >= 1500) // 2008 or later
// Remember that usage of defined() in a #define is undefined by the standard.
// a user reported that in 64-bit mode, MSVC doesn't care to define _M_IX86_FP.
#if (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) || defined(_M_X64)
#define EIGEN_SSE2_ON_MSVC_2008_OR_LATER
#endif
#endif
#endif
// FIXME: this check should not be against __QNXNTO__, which is also defined
// while compiling with GCC for QNX target. Better solution is welcome!
#if defined(__GNUC__) && !defined(__QNXNTO__)
#define EIGEN_GNUC_AT_LEAST(x,y) ((__GNUC__>=x && __GNUC_MINOR__>=y) || __GNUC__>x)
#else
#define EIGEN_GNUC_AT_LEAST(x,y) 0
#endif
// Remember that usage of defined() in a #define is undefined by the standard
#if (defined __SSE2__) && ( (!defined __GNUC__) || EIGEN_GNUC_AT_LEAST(4,2) )
#define EIGEN_SSE2_BUT_NOT_OLD_GCC
#endif
#if !defined(EIGEN_DONT_VECTORIZE) && !defined(EIGEN_DONT_ALIGN)
#if defined (EIGEN_SSE2_BUT_NOT_OLD_GCC) || defined(EIGEN_SSE2_ON_MSVC_2008_OR_LATER)
#define EIGEN_VECTORIZE
#define EIGEN_VECTORIZE_SSE
#include <emmintrin.h>
#include <xmmintrin.h>
#ifdef __SSE3__
#include <pmmintrin.h>
#endif
#ifdef __SSSE3__
#include <tmmintrin.h>
#endif
#elif defined __ALTIVEC__
#define EIGEN_VECTORIZE
#define EIGEN_VECTORIZE_ALTIVEC
#include <altivec.h>
// We need to #undef all these ugly tokens defined in <altivec.h>
// => use __vector instead of vector
#undef bool
#undef vector
#undef pixel
#endif
#endif
#include <cstdlib>
#include <cmath>
#include <complex>
#include <cassert>
#include <functional>
#include <iosfwd>
#include <cstring>
#include <string>
#include <limits>
#if (defined(_CPPUNWIND) || defined(__EXCEPTIONS)) && !defined(EIGEN_NO_EXCEPTIONS)
#define EIGEN_EXCEPTIONS
#endif
#ifdef EIGEN_EXCEPTIONS
#include <new>
#endif
// this needs to be done after all possible windows C header includes and before any Eigen source includes
// (system C++ includes are supposed to be able to deal with this already):
// windows.h defines min and max macros which would make Eigen fail to compile.
#if defined(min) || defined(max)
#error The preprocessor symbols 'min' or 'max' are defined. If you are compiling on Windows, do #define NOMINMAX to prevent windows.h from defining these symbols.
#endif
namespace Eigen {
// we use size_t frequently and we'll never remember to prepend it with std:: everytime just to
// ensure QNX/QCC support
using std::size_t;
/** \defgroup Core_Module Core module
* This is the main module of Eigen providing dense matrix and vector support
* (both fixed and dynamic size) with all the features corresponding to a BLAS library
* and much more...
*
* \code
* #include <Eigen/Core>
* \endcode
*/
#include "src/Core/util/Macros.h"
#include "src/Core/util/Constants.h"
#include "src/Core/util/ForwardDeclarations.h"
#include "src/Core/util/Meta.h"
#include "src/Core/util/XprHelper.h"
#include "src/Core/util/StaticAssert.h"
#include "src/Core/util/Memory.h"
#include "src/Core/NumTraits.h"
#include "src/Core/MathFunctions.h"
#include "src/Core/GenericPacketMath.h"
#if defined EIGEN_VECTORIZE_SSE
#include "src/Core/arch/SSE/PacketMath.h"
#elif defined EIGEN_VECTORIZE_ALTIVEC
#include "src/Core/arch/AltiVec/PacketMath.h"
#endif
#ifndef EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD
#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 16
#endif
#include "src/Core/Functors.h"
#include "src/Core/MatrixBase.h"
#include "src/Core/Coeffs.h"
#ifndef EIGEN_PARSED_BY_DOXYGEN // work around Doxygen bug triggered by Assign.h r814874
// at least confirmed with Doxygen 1.5.5 and 1.5.6
#include "src/Core/Assign.h"
#endif
#include "src/Core/MatrixStorage.h"
#include "src/Core/NestByValue.h"
#include "src/Core/Flagged.h"
#include "src/Core/Matrix.h"
#include "src/Core/Cwise.h"
#include "src/Core/CwiseBinaryOp.h"
#include "src/Core/CwiseUnaryOp.h"
#include "src/Core/CwiseNullaryOp.h"
#include "src/Core/Dot.h"
#include "src/Core/Product.h"
#include "src/Core/DiagonalProduct.h"
#include "src/Core/SolveTriangular.h"
#include "src/Core/MapBase.h"
#include "src/Core/Map.h"
#include "src/Core/Block.h"
#include "src/Core/Minor.h"
#include "src/Core/Transpose.h"
#include "src/Core/DiagonalMatrix.h"
#include "src/Core/DiagonalCoeffs.h"
#include "src/Core/Sum.h"
#include "src/Core/Redux.h"
#include "src/Core/Visitor.h"
#include "src/Core/Fuzzy.h"
#include "src/Core/IO.h"
#include "src/Core/Swap.h"
#include "src/Core/CommaInitializer.h"
#include "src/Core/Part.h"
#include "src/Core/CacheFriendlyProduct.h"
} // namespace Eigen
#include "src/Core/util/EnableMSVCWarnings.h"
#endif // EIGEN_CORE_H

8
ground/openpilotgcs/src/libs/eigen/Eigen/Dense Normal file
View File

@ -0,0 +1,8 @@
#include "Core"
#include "Array"
#include "LU"
#include "Cholesky"
#include "QR"
#include "SVD"
#include "Geometry"
#include "LeastSquares"

2
ground/openpilotgcs/src/libs/eigen/Eigen/Eigen Normal file
View File

@ -0,0 +1,2 @@
#include "Dense"
#include "Sparse"

51
ground/openpilotgcs/src/libs/eigen/Eigen/Geometry Normal file
View File

@ -0,0 +1,51 @@
#ifndef EIGEN_GEOMETRY_MODULE_H
#define EIGEN_GEOMETRY_MODULE_H
#include "Core"
#include "src/Core/util/DisableMSVCWarnings.h"
#include "Array"
#include <limits>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
namespace Eigen {
/** \defgroup Geometry_Module Geometry module
*
* \nonstableyet
*
* This module provides support for:
* - fixed-size homogeneous transformations
* - translation, scaling, 2D and 3D rotations
* - quaternions
* - \ref MatrixBase::cross() "cross product"
* - \ref MatrixBase::unitOrthogonal() "orthognal vector generation"
* - some linear components: parametrized-lines and hyperplanes
*
* \code
* #include <Eigen/Geometry>
* \endcode
*/
#include "src/Geometry/OrthoMethods.h"
#include "src/Geometry/RotationBase.h"
#include "src/Geometry/Rotation2D.h"
#include "src/Geometry/Quaternion.h"
#include "src/Geometry/AngleAxis.h"
#include "src/Geometry/EulerAngles.h"
#include "src/Geometry/Transform.h"
#include "src/Geometry/Translation.h"
#include "src/Geometry/Scaling.h"
#include "src/Geometry/Hyperplane.h"
#include "src/Geometry/ParametrizedLine.h"
#include "src/Geometry/AlignedBox.h"
} // namespace Eigen
#include "src/Core/util/EnableMSVCWarnings.h"
#endif // EIGEN_GEOMETRY_MODULE_H

29
ground/openpilotgcs/src/libs/eigen/Eigen/LU Normal file
View File

@ -0,0 +1,29 @@
#ifndef EIGEN_LU_MODULE_H
#define EIGEN_LU_MODULE_H
#include "Core"
#include "src/Core/util/DisableMSVCWarnings.h"
namespace Eigen {
/** \defgroup LU_Module LU module
* This module includes %LU decomposition and related notions such as matrix inversion and determinant.
* This module defines the following MatrixBase methods:
* - MatrixBase::inverse()
* - MatrixBase::determinant()
*
* \code
* #include <Eigen/LU>
* \endcode
*/
#include "src/LU/LU.h"
#include "src/LU/Determinant.h"
#include "src/LU/Inverse.h"
} // namespace Eigen
#include "src/Core/util/EnableMSVCWarnings.h"
#endif // EIGEN_LU_MODULE_H

27
ground/openpilotgcs/src/libs/eigen/Eigen/LeastSquares Normal file
View File

@ -0,0 +1,27 @@
#ifndef EIGEN_REGRESSION_MODULE_H
#define EIGEN_REGRESSION_MODULE_H
#include "Core"
#include "src/Core/util/DisableMSVCWarnings.h"
#include "QR"
#include "Geometry"
namespace Eigen {
/** \defgroup LeastSquares_Module LeastSquares module
* This module provides linear regression and related features.
*
* \code
* #include <Eigen/LeastSquares>
* \endcode
*/
#include "src/LeastSquares/LeastSquares.h"
} // namespace Eigen
#include "src/Core/util/EnableMSVCWarnings.h"
#endif // EIGEN_REGRESSION_MODULE_H

168
ground/openpilotgcs/src/libs/eigen/Eigen/NewStdVector Normal file
View File

@ -0,0 +1,168 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2009 Hauke Heibel <hauke.heibel@googlemail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_STDVECTOR_MODULE_H
#define EIGEN_STDVECTOR_MODULE_H
#include "Core"
#include <vector>
namespace Eigen {
// This one is needed to prevent reimplementing the whole std::vector.
template <class T>
class aligned_allocator_indirection : public aligned_allocator<T>
{
public:
typedef std::size_t size_type;
typedef std::ptrdiff_t difference_type;
typedef T* pointer;
typedef const T* const_pointer;
typedef T& reference;
typedef const T& const_reference;
typedef T value_type;
template<class U>
struct rebind
{
typedef aligned_allocator_indirection<U> other;
};
aligned_allocator_indirection() throw() {}
aligned_allocator_indirection(const aligned_allocator_indirection& ) throw() : aligned_allocator<T>() {}
aligned_allocator_indirection(const aligned_allocator<T>& ) throw() {}
template<class U>
aligned_allocator_indirection(const aligned_allocator_indirection<U>& ) throw() {}
template<class U>
aligned_allocator_indirection(const aligned_allocator<U>& ) throw() {}
~aligned_allocator_indirection() throw() {}
};
#ifdef _MSC_VER
// sometimes, MSVC detects, at compile time, that the argument x
// in std::vector::resize(size_t s,T x) won't be aligned and generate an error
// even if this function is never called. Whence this little wrapper.
#define EIGEN_WORKAROUND_MSVC_STD_VECTOR(T) Eigen::ei_workaround_msvc_std_vector<T>
template<typename T> struct ei_workaround_msvc_std_vector : public T
{
inline ei_workaround_msvc_std_vector() : T() {}
inline ei_workaround_msvc_std_vector(const T& other) : T(other) {}
inline operator T& () { return *static_cast<T*>(this); }
inline operator const T& () const { return *static_cast<const T*>(this); }
template<typename OtherT>
inline T& operator=(const OtherT& other)
{ T::operator=(other); return *this; }
inline ei_workaround_msvc_std_vector& operator=(const ei_workaround_msvc_std_vector& other)
{ T::operator=(other); return *this; }
};
#else
#define EIGEN_WORKAROUND_MSVC_STD_VECTOR(T) T
#endif
}
namespace std {
#define EIGEN_STD_VECTOR_SPECIALIZATION_BODY \
public: \
typedef T value_type; \
typedef typename vector_base::allocator_type allocator_type; \
typedef typename vector_base::size_type size_type; \
typedef typename vector_base::iterator iterator; \
typedef typename vector_base::const_iterator const_iterator; \
explicit vector(const allocator_type& a = allocator_type()) : vector_base(a) {} \
template<typename InputIterator> \
vector(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) \
: vector_base(first, last, a) {} \
vector(const vector& c) : vector_base(c) {} \
explicit vector(size_type num, const value_type& val = value_type()) : vector_base(num, val) {} \
vector(iterator start, iterator end) : vector_base(start, end) {} \
vector& operator=(const vector& x) { \
vector_base::operator=(x); \
return *this; \
}
template<typename T>
class vector<T,Eigen::aligned_allocator<T> >
: public vector<EIGEN_WORKAROUND_MSVC_STD_VECTOR(T),
Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STD_VECTOR(T)> >
{
typedef vector<EIGEN_WORKAROUND_MSVC_STD_VECTOR(T),
Eigen::aligned_allocator_indirection<EIGEN_WORKAROUND_MSVC_STD_VECTOR(T)> > vector_base;
EIGEN_STD_VECTOR_SPECIALIZATION_BODY
void resize(size_type new_size)
{ resize(new_size, T()); }
#if defined(_VECTOR_)
// workaround MSVC std::vector implementation
void resize(size_type new_size, const value_type& x)
{
if (vector_base::size() < new_size)
vector_base::_Insert_n(vector_base::end(), new_size - vector_base::size(), x);
else if (new_size < vector_base::size())
vector_base::erase(vector_base::begin() + new_size, vector_base::end());
}
void push_back(const value_type& x)
{ vector_base::push_back(x); }
using vector_base::insert;
iterator insert(const_iterator position, const value_type& x)
{ return vector_base::insert(position,x); }
void insert(const_iterator position, size_type new_size, const value_type& x)
{ vector_base::insert(position, new_size, x); }
#elif defined(_GLIBCXX_VECTOR) && EIGEN_GNUC_AT_LEAST(4,2)
// workaround GCC std::vector implementation
void resize(size_type new_size, const value_type& x)
{
if (new_size < vector_base::size())
vector_base::_M_erase_at_end(this->_M_impl._M_start + new_size);
else
vector_base::insert(vector_base::end(), new_size - vector_base::size(), x);
}
#elif defined(_GLIBCXX_VECTOR) && (!EIGEN_GNUC_AT_LEAST(4,1))
// Note that before gcc-4.1 we already have: std::vector::resize(size_type,const T&),
// no no need to workaround !
using vector_base::resize;
#else
// either GCC 4.1 or non-GCC
// default implementation which should always work.
void resize(size_type new_size, const value_type& x)
{
if (new_size < vector_base::size())
vector_base::erase(vector_base::begin() + new_size, vector_base::end());
else if (new_size > vector_base::size())
vector_base::insert(vector_base::end(), new_size - vector_base::size(), x);
}
#endif
};
}
#endif // EIGEN_STDVECTOR_MODULE_H

73
ground/openpilotgcs/src/libs/eigen/Eigen/QR Normal file
View File

@ -0,0 +1,73 @@
#ifndef EIGEN_QR_MODULE_H
#define EIGEN_QR_MODULE_H
#include "Core"
#include "src/Core/util/DisableMSVCWarnings.h"
#include "Cholesky"
// Note that EIGEN_HIDE_HEAVY_CODE has to be defined per module
#if (defined EIGEN_EXTERN_INSTANTIATIONS) && (EIGEN_EXTERN_INSTANTIATIONS>=2)
#ifndef EIGEN_HIDE_HEAVY_CODE
#define EIGEN_HIDE_HEAVY_CODE
#endif
#elif defined EIGEN_HIDE_HEAVY_CODE
#undef EIGEN_HIDE_HEAVY_CODE
#endif
namespace Eigen {
/** \defgroup QR_Module QR module
*
* \nonstableyet
*
* This module mainly provides QR decomposition and an eigen value solver.
* This module also provides some MatrixBase methods, including:
* - MatrixBase::qr(),
* - MatrixBase::eigenvalues(),
* - MatrixBase::operatorNorm()
*
* \code
* #include <Eigen/QR>
* \endcode
*/
#include "src/QR/QR.h"
#include "src/QR/Tridiagonalization.h"
#include "src/QR/EigenSolver.h"
#include "src/QR/SelfAdjointEigenSolver.h"
#include "src/QR/HessenbergDecomposition.h"
// declare all classes for a given matrix type
#define EIGEN_QR_MODULE_INSTANTIATE_TYPE(MATRIXTYPE,PREFIX) \
PREFIX template class QR<MATRIXTYPE>; \
PREFIX template class Tridiagonalization<MATRIXTYPE>; \
PREFIX template class HessenbergDecomposition<MATRIXTYPE>; \
PREFIX template class SelfAdjointEigenSolver<MATRIXTYPE>
// removed because it does not support complex yet
// PREFIX template class EigenSolver<MATRIXTYPE>
// declare all class for all types
#define EIGEN_QR_MODULE_INSTANTIATE(PREFIX) \
EIGEN_QR_MODULE_INSTANTIATE_TYPE(Matrix2f,PREFIX); \
EIGEN_QR_MODULE_INSTANTIATE_TYPE(Matrix2d,PREFIX); \
EIGEN_QR_MODULE_INSTANTIATE_TYPE(Matrix3f,PREFIX); \
EIGEN_QR_MODULE_INSTANTIATE_TYPE(Matrix3d,PREFIX); \
EIGEN_QR_MODULE_INSTANTIATE_TYPE(Matrix4f,PREFIX); \
EIGEN_QR_MODULE_INSTANTIATE_TYPE(Matrix4d,PREFIX); \
EIGEN_QR_MODULE_INSTANTIATE_TYPE(MatrixXf,PREFIX); \
EIGEN_QR_MODULE_INSTANTIATE_TYPE(MatrixXd,PREFIX); \
EIGEN_QR_MODULE_INSTANTIATE_TYPE(MatrixXcf,PREFIX); \
EIGEN_QR_MODULE_INSTANTIATE_TYPE(MatrixXcd,PREFIX)
#ifdef EIGEN_EXTERN_INSTANTIATIONS
EIGEN_QR_MODULE_INSTANTIATE(extern);
#endif // EIGEN_EXTERN_INSTANTIATIONS
} // namespace Eigen
#include "src/Core/util/EnableMSVCWarnings.h"
#endif // EIGEN_QR_MODULE_H

49
ground/openpilotgcs/src/libs/eigen/Eigen/QtAlignedMalloc Normal file
View File

@ -0,0 +1,49 @@
#ifndef EIGEN_QTMALLOC_MODULE_H
#define EIGEN_QTMALLOC_MODULE_H
#if (!EIGEN_MALLOC_ALREADY_ALIGNED)
#ifdef QVECTOR_H
#error You must include <Eigen/QtAlignedMalloc> before <QtCore/QVector>.
#endif
#ifdef Q_DECL_IMPORT
#define Q_DECL_IMPORT_ORIG Q_DECL_IMPORT
#undef Q_DECL_IMPORT
#define Q_DECL_IMPORT
#else
#define Q_DECL_IMPORT
#endif
#include "Core"
#include <QtCore/QVector>
inline void *qMalloc(size_t size)
{
return Eigen::ei_aligned_malloc(size);
}
inline void qFree(void *ptr)
{
Eigen::ei_aligned_free(ptr);
}
inline void *qRealloc(void *ptr, size_t size)
{
void* newPtr = Eigen::ei_aligned_malloc(size);
memcpy(newPtr, ptr, size);
Eigen::ei_aligned_free(ptr);
return newPtr;
}
#endif
#ifdef Q_DECL_IMPORT_ORIG
#define Q_DECL_IMPORT Q_DECL_IMPORT_ORIG
#undef Q_DECL_IMPORT_ORIG
#else
#undef Q_DECL_IMPORT
#endif
#endif // EIGEN_QTMALLOC_MODULE_H

29
ground/openpilotgcs/src/libs/eigen/Eigen/SVD Normal file
View File

@ -0,0 +1,29 @@
#ifndef EIGEN_SVD_MODULE_H
#define EIGEN_SVD_MODULE_H
#include "Core"
#include "src/Core/util/DisableMSVCWarnings.h"
namespace Eigen {
/** \defgroup SVD_Module SVD module
*
* \nonstableyet
*
* This module provides SVD decomposition for (currently) real matrices.
* This decomposition is accessible via the following MatrixBase method:
* - MatrixBase::svd()
*
* \code
* #include <Eigen/SVD>
* \endcode
*/
#include "src/SVD/SVD.h"
} // namespace Eigen
#include "src/Core/util/EnableMSVCWarnings.h"
#endif // EIGEN_SVD_MODULE_H

132
ground/openpilotgcs/src/libs/eigen/Eigen/Sparse Normal file
View File

@ -0,0 +1,132 @@
#ifndef EIGEN_SPARSE_MODULE_H
#define EIGEN_SPARSE_MODULE_H
#include "Core"
#include "src/Core/util/DisableMSVCWarnings.h"
#include <vector>
#include <map>
#include <cstdlib>
#include <cstring>
#include <algorithm>
#ifdef EIGEN_GOOGLEHASH_SUPPORT
#include <google/dense_hash_map>
#endif
#ifdef EIGEN_CHOLMOD_SUPPORT
extern "C" {
#include "cholmod.h"
}
#endif
#ifdef EIGEN_TAUCS_SUPPORT
// taucs.h declares a lot of mess
#define isnan
#define finite
#define isinf
extern "C" {
#include "taucs.h"
}
#undef isnan
#undef finite
#undef isinf
#ifdef min
#undef min
#endif
#ifdef max
#undef max
#endif
#ifdef complex
#undef complex
#endif
#endif
#ifdef EIGEN_SUPERLU_SUPPORT
typedef int int_t;
#include "superlu/slu_Cnames.h"
#include "superlu/supermatrix.h"
#include "superlu/slu_util.h"
namespace SuperLU_S {
#include "superlu/slu_sdefs.h"
}
namespace SuperLU_D {
#include "superlu/slu_ddefs.h"
}
namespace SuperLU_C {
#include "superlu/slu_cdefs.h"
}
namespace SuperLU_Z {
#include "superlu/slu_zdefs.h"
}
namespace Eigen { struct SluMatrix; }
#endif
#ifdef EIGEN_UMFPACK_SUPPORT
#include "umfpack.h"
#endif
namespace Eigen {
/** \defgroup Sparse_Module Sparse module
*
* \nonstableyet
*
* See the \ref TutorialSparse "Sparse tutorial"
*
* \code
* #include <Eigen/QR>
* \endcode
*/
#include "src/Sparse/SparseUtil.h"
#include "src/Sparse/SparseMatrixBase.h"
#include "src/Sparse/CompressedStorage.h"
#include "src/Sparse/AmbiVector.h"
#include "src/Sparse/RandomSetter.h"
#include "src/Sparse/SparseBlock.h"
#include "src/Sparse/SparseMatrix.h"
#include "src/Sparse/DynamicSparseMatrix.h"
#include "src/Sparse/MappedSparseMatrix.h"
#include "src/Sparse/SparseVector.h"
#include "src/Sparse/CoreIterators.h"
#include "src/Sparse/SparseTranspose.h"
#include "src/Sparse/SparseCwise.h"
#include "src/Sparse/SparseCwiseUnaryOp.h"
#include "src/Sparse/SparseCwiseBinaryOp.h"
#include "src/Sparse/SparseDot.h"
#include "src/Sparse/SparseAssign.h"
#include "src/Sparse/SparseRedux.h"
#include "src/Sparse/SparseFuzzy.h"
#include "src/Sparse/SparseFlagged.h"
#include "src/Sparse/SparseProduct.h"
#include "src/Sparse/SparseDiagonalProduct.h"
#include "src/Sparse/TriangularSolver.h"
#include "src/Sparse/SparseLLT.h"
#include "src/Sparse/SparseLDLT.h"
#include "src/Sparse/SparseLU.h"
#ifdef EIGEN_CHOLMOD_SUPPORT
# include "src/Sparse/CholmodSupport.h"
#endif
#ifdef EIGEN_TAUCS_SUPPORT
# include "src/Sparse/TaucsSupport.h"
#endif
#ifdef EIGEN_SUPERLU_SUPPORT
# include "src/Sparse/SuperLUSupport.h"
#endif
#ifdef EIGEN_UMFPACK_SUPPORT
# include "src/Sparse/UmfPackSupport.h"
#endif
} // namespace Eigen
#include "src/Core/util/EnableMSVCWarnings.h"
#endif // EIGEN_SPARSE_MODULE_H

147
ground/openpilotgcs/src/libs/eigen/Eigen/StdVector Normal file
View File

@ -0,0 +1,147 @@
#ifdef EIGEN_USE_NEW_STDVECTOR
#include "NewStdVector"
#else
#ifndef EIGEN_STDVECTOR_MODULE_H
#define EIGEN_STDVECTOR_MODULE_H
#if defined(_GLIBCXX_VECTOR) || defined(_VECTOR_)
#error you must include <Eigen/StdVector> before <vector>. Also note that <Eigen/Sparse> includes <vector>, so it must be included after <Eigen/StdVector> too.
#endif
#ifndef EIGEN_GNUC_AT_LEAST
#ifdef __GNUC__
#define EIGEN_GNUC_AT_LEAST(x,y) ((__GNUC__>=x && __GNUC_MINOR__>=y) || __GNUC__>x)
#else
#define EIGEN_GNUC_AT_LEAST(x,y) 0
#endif
#endif
#define vector std_vector
#include <vector>
#undef vector
namespace Eigen {
template<typename T> class aligned_allocator;
// meta programming to determine if a class has a given member
struct ei_does_not_have_aligned_operator_new_marker_sizeof {int a[1];};
struct ei_has_aligned_operator_new_marker_sizeof {int a[2];};
template<typename ClassType>
struct ei_has_aligned_operator_new {
template<typename T>
static ei_has_aligned_operator_new_marker_sizeof
test(T const *, typename T::ei_operator_new_marker_type const * = 0);
static ei_does_not_have_aligned_operator_new_marker_sizeof
test(...);
// note that the following indirection is needed for gcc-3.3
enum {ret = sizeof(test(static_cast<ClassType*>(0)))
== sizeof(ei_has_aligned_operator_new_marker_sizeof) };
};
#ifdef _MSC_VER
// sometimes, MSVC detects, at compile time, that the argument x
// in std::vector::resize(size_t s,T x) won't be aligned and generate an error
// even if this function is never called. Whence this little wrapper.
#define _EIGEN_WORKAROUND_MSVC_STD_VECTOR(T) Eigen::ei_workaround_msvc_std_vector<T>
template<typename T> struct ei_workaround_msvc_std_vector : public T
{
inline ei_workaround_msvc_std_vector() : T() {}
inline ei_workaround_msvc_std_vector(const T& other) : T(other) {}
inline operator T& () { return *static_cast<T*>(this); }
inline operator const T& () const { return *static_cast<const T*>(this); }
template<typename OtherT>
inline T& operator=(const OtherT& other)
{ T::operator=(other); return *this; }
inline ei_workaround_msvc_std_vector& operator=(const ei_workaround_msvc_std_vector& other)
{ T::operator=(other); return *this; }
};
#else
#define _EIGEN_WORKAROUND_MSVC_STD_VECTOR(T) T
#endif
}
namespace std {
#define EIGEN_STD_VECTOR_SPECIALIZATION_BODY \
public: \
typedef T value_type; \
typedef typename vector_base::allocator_type allocator_type; \
typedef typename vector_base::size_type size_type; \
typedef typename vector_base::iterator iterator; \
explicit vector(const allocator_type& __a = allocator_type()) : vector_base(__a) {} \
vector(const vector& c) : vector_base(c) {} \
vector(size_type num, const value_type& val = value_type()) : vector_base(num, val) {} \
vector(iterator start, iterator end) : vector_base(start, end) {} \
vector& operator=(const vector& __x) { \
vector_base::operator=(__x); \
return *this; \
}
template<typename T,
typename AllocT = std::allocator<T>,
bool HasAlignedNew = Eigen::ei_has_aligned_operator_new<T>::ret>
class vector : public std::std_vector<T,AllocT>
{
typedef std_vector<T, AllocT> vector_base;
EIGEN_STD_VECTOR_SPECIALIZATION_BODY
};
template<typename T,typename DummyAlloc>
class vector<T,DummyAlloc,true>
: public std::std_vector<_EIGEN_WORKAROUND_MSVC_STD_VECTOR(T),
Eigen::aligned_allocator<_EIGEN_WORKAROUND_MSVC_STD_VECTOR(T)> >
{
typedef std_vector<_EIGEN_WORKAROUND_MSVC_STD_VECTOR(T),
Eigen::aligned_allocator<_EIGEN_WORKAROUND_MSVC_STD_VECTOR(T)> > vector_base;
EIGEN_STD_VECTOR_SPECIALIZATION_BODY
void resize(size_type __new_size)
{ resize(__new_size, T()); }
#if defined(_VECTOR_)
// workaround MSVC std::vector implementation
void resize(size_type __new_size, const value_type& __x)
{
if (vector_base::size() < __new_size)
vector_base::_Insert_n(vector_base::end(), __new_size - vector_base::size(), __x);
else if (__new_size < vector_base::size())
vector_base::erase(vector_base::begin() + __new_size, vector_base::end());
}
#elif defined(_GLIBCXX_VECTOR) && EIGEN_GNUC_AT_LEAST(4,2)
// workaround GCC std::vector implementation
void resize(size_type __new_size, const value_type& __x)
{
if (__new_size < vector_base::size())
vector_base::_M_erase_at_end(this->_M_impl._M_start + __new_size);
else
vector_base::insert(vector_base::end(), __new_size - vector_base::size(), __x);
}
#elif defined(_GLIBCXX_VECTOR) && EIGEN_GNUC_AT_LEAST(4,1)
void resize(size_type __new_size, const value_type& __x)
{
if (__new_size < vector_base::size())
vector_base::erase(vector_base::begin() + __new_size, vector_base::end());
else
vector_base::insert(vector_base::end(), __new_size - vector_base::size(), __x);
}
#else
// Before gcc-4.1 we already have: std::vector::resize(size_type,const T&),
// so no need for a workaround !
using vector_base::resize;
#endif
};
}
#endif // EIGEN_STDVECTOR_MODULE_H
#endif // EIGEN_USE_NEW_STDVECTOR

145
ground/openpilotgcs/src/libs/eigen/Eigen/src/Array/BooleanRedux.h Normal file
View File

@ -0,0 +1,145 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_ALLANDANY_H
#define EIGEN_ALLANDANY_H
template<typename Derived, int UnrollCount>
struct ei_all_unroller
{
enum {
col = (UnrollCount-1) / Derived::RowsAtCompileTime,
row = (UnrollCount-1) % Derived::RowsAtCompileTime
};
inline static bool run(const Derived &mat)
{
return ei_all_unroller<Derived, UnrollCount-1>::run(mat) && mat.coeff(row, col);
}
};
template<typename Derived>
struct ei_all_unroller<Derived, 1>
{
inline static bool run(const Derived &mat) { return mat.coeff(0, 0); }
};
template<typename Derived>
struct ei_all_unroller<Derived, Dynamic>
{
inline static bool run(const Derived &) { return false; }
};
template<typename Derived, int UnrollCount>
struct ei_any_unroller
{
enum {
col = (UnrollCount-1) / Derived::RowsAtCompileTime,
row = (UnrollCount-1) % Derived::RowsAtCompileTime
};
inline static bool run(const Derived &mat)
{
return ei_any_unroller<Derived, UnrollCount-1>::run(mat) || mat.coeff(row, col);
}
};
template<typename Derived>
struct ei_any_unroller<Derived, 1>
{
inline static bool run(const Derived &mat) { return mat.coeff(0, 0); }
};
template<typename Derived>
struct ei_any_unroller<Derived, Dynamic>
{
inline static bool run(const Derived &) { return false; }
};
/** \array_module
*
* \returns true if all coefficients are true
*
* \addexample CwiseAll \label How to check whether a point is inside a box (using operator< and all())
*
* Example: \include MatrixBase_all.cpp
* Output: \verbinclude MatrixBase_all.out
*
* \sa MatrixBase::any(), Cwise::operator<()
*/
template<typename Derived>
inline bool MatrixBase<Derived>::all() const
{
const bool unroll = SizeAtCompileTime * (CoeffReadCost + NumTraits<Scalar>::AddCost)
<= EIGEN_UNROLLING_LIMIT;
if(unroll)
return ei_all_unroller<Derived,
unroll ? int(SizeAtCompileTime) : Dynamic
>::run(derived());
else
{
for(int j = 0; j < cols(); ++j)
for(int i = 0; i < rows(); ++i)
if (!coeff(i, j)) return false;
return true;
}
}
/** \array_module
*
* \returns true if at least one coefficient is true
*
* \sa MatrixBase::all()
*/
template<typename Derived>
inline bool MatrixBase<Derived>::any() const
{
const bool unroll = SizeAtCompileTime * (CoeffReadCost + NumTraits<Scalar>::AddCost)
<= EIGEN_UNROLLING_LIMIT;
if(unroll)
return ei_any_unroller<Derived,
unroll ? int(SizeAtCompileTime) : Dynamic
>::run(derived());
else
{
for(int j = 0; j < cols(); ++j)
for(int i = 0; i < rows(); ++i)
if (coeff(i, j)) return true;
return false;
}
}
/** \array_module
*
* \returns the number of coefficients which evaluate to true
*
* \sa MatrixBase::all(), MatrixBase::any()
*/
template<typename Derived>
inline int MatrixBase<Derived>::count() const
{
return this->cast<bool>().template cast<int>().sum();
}
#endif // EIGEN_ALLANDANY_H

6
ground/openpilotgcs/src/libs/eigen/Eigen/src/Array/CMakeLists.txt Normal file
View File

@ -0,0 +1,6 @@
FILE(GLOB Eigen_Array_SRCS "*.h")
INSTALL(FILES
${Eigen_Array_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Array
)

453
ground/openpilotgcs/src/libs/eigen/Eigen/src/Array/CwiseOperators.h Normal file
View File

@ -0,0 +1,453 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_ARRAY_CWISE_OPERATORS_H
#define EIGEN_ARRAY_CWISE_OPERATORS_H
// -- unary operators --
/** \array_module
*
* \returns an expression of the coefficient-wise square root of *this.
*
* Example: \include Cwise_sqrt.cpp
* Output: \verbinclude Cwise_sqrt.out
*
* \sa pow(), square()
*/
template<typename ExpressionType>
inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_sqrt_op)
Cwise<ExpressionType>::sqrt() const
{
return _expression();
}
/** \array_module
*
* \returns an expression of the coefficient-wise exponential of *this.
*
* Example: \include Cwise_exp.cpp
* Output: \verbinclude Cwise_exp.out
*
* \sa pow(), log(), sin(), cos()
*/
template<typename ExpressionType>
inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_exp_op)
Cwise<ExpressionType>::exp() const
{
return _expression();
}
/** \array_module
*
* \returns an expression of the coefficient-wise logarithm of *this.
*
* Example: \include Cwise_log.cpp
* Output: \verbinclude Cwise_log.out
*
* \sa exp()
*/
template<typename ExpressionType>
inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_log_op)
Cwise<ExpressionType>::log() const
{
return _expression();
}
/** \array_module
*
* \returns an expression of the coefficient-wise cosine of *this.
*
* Example: \include Cwise_cos.cpp
* Output: \verbinclude Cwise_cos.out
*
* \sa sin(), exp()
*/
template<typename ExpressionType>
inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_cos_op)
Cwise<ExpressionType>::cos() const
{
return _expression();
}
/** \array_module
*
* \returns an expression of the coefficient-wise sine of *this.
*
* Example: \include Cwise_sin.cpp
* Output: \verbinclude Cwise_sin.out
*
* \sa cos(), exp()
*/
template<typename ExpressionType>
inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_sin_op)
Cwise<ExpressionType>::sin() const
{
return _expression();
}
/** \array_module
*
* \returns an expression of the coefficient-wise power of *this to the given exponent.
*
* Example: \include Cwise_pow.cpp
* Output: \verbinclude Cwise_pow.out
*
* \sa exp(), log()
*/
template<typename ExpressionType>
inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_pow_op)
Cwise<ExpressionType>::pow(const Scalar& exponent) const
{
return EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_pow_op)(_expression(), ei_scalar_pow_op<Scalar>(exponent));
}
/** \array_module
*
* \returns an expression of the coefficient-wise inverse of *this.
*
* Example: \include Cwise_inverse.cpp
* Output: \verbinclude Cwise_inverse.out
*
* \sa operator/(), operator*()
*/
template<typename ExpressionType>
inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_inverse_op)
Cwise<ExpressionType>::inverse() const
{
return _expression();
}
/** \array_module
*
* \returns an expression of the coefficient-wise square of *this.
*
* Example: \include Cwise_square.cpp
* Output: \verbinclude Cwise_square.out
*
* \sa operator/(), operator*(), abs2()
*/
template<typename ExpressionType>
inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_square_op)
Cwise<ExpressionType>::square() const
{
return _expression();
}
/** \array_module
*
* \returns an expression of the coefficient-wise cube of *this.
*
* Example: \include Cwise_cube.cpp
* Output: \verbinclude Cwise_cube.out
*
* \sa square(), pow()
*/
template<typename ExpressionType>
inline const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_cube_op)
Cwise<ExpressionType>::cube() const
{
return _expression();
}
// -- binary operators --
/** \array_module
*
* \returns an expression of the coefficient-wise \< operator of *this and \a other
*
* Example: \include Cwise_less.cpp
* Output: \verbinclude Cwise_less.out
*
* \sa MatrixBase::all(), MatrixBase::any(), operator>(), operator<=()
*/
template<typename ExpressionType>
template<typename OtherDerived>
inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::less)
Cwise<ExpressionType>::operator<(const MatrixBase<OtherDerived> &other) const
{
return EIGEN_CWISE_BINOP_RETURN_TYPE(std::less)(_expression(), other.derived());
}
/** \array_module
*
* \returns an expression of the coefficient-wise \<= operator of *this and \a other
*
* Example: \include Cwise_less_equal.cpp
* Output: \verbinclude Cwise_less_equal.out
*
* \sa MatrixBase::all(), MatrixBase::any(), operator>=(), operator<()
*/
template<typename ExpressionType>
template<typename OtherDerived>
inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::less_equal)
Cwise<ExpressionType>::operator<=(const MatrixBase<OtherDerived> &other) const
{
return EIGEN_CWISE_BINOP_RETURN_TYPE(std::less_equal)(_expression(), other.derived());
}
/** \array_module
*
* \returns an expression of the coefficient-wise \> operator of *this and \a other
*
* Example: \include Cwise_greater.cpp
* Output: \verbinclude Cwise_greater.out
*
* \sa MatrixBase::all(), MatrixBase::any(), operator>=(), operator<()
*/
template<typename ExpressionType>
template<typename OtherDerived>
inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater)
Cwise<ExpressionType>::operator>(const MatrixBase<OtherDerived> &other) const
{
return EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater)(_expression(), other.derived());
}
/** \array_module
*
* \returns an expression of the coefficient-wise \>= operator of *this and \a other
*
* Example: \include Cwise_greater_equal.cpp
* Output: \verbinclude Cwise_greater_equal.out
*
* \sa MatrixBase::all(), MatrixBase::any(), operator>(), operator<=()
*/
template<typename ExpressionType>
template<typename OtherDerived>
inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater_equal)
Cwise<ExpressionType>::operator>=(const MatrixBase<OtherDerived> &other) const
{
return EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater_equal)(_expression(), other.derived());
}
/** \array_module
*
* \returns an expression of the coefficient-wise == operator of *this and \a other
*
* \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
* In order to check for equality between two vectors or matrices with floating-point coefficients, it is
* generally a far better idea to use a fuzzy comparison as provided by MatrixBase::isApprox() and
* MatrixBase::isMuchSmallerThan().
*
* Example: \include Cwise_equal_equal.cpp
* Output: \verbinclude Cwise_equal_equal.out
*
* \sa MatrixBase::all(), MatrixBase::any(), MatrixBase::isApprox(), MatrixBase::isMuchSmallerThan()
*/
template<typename ExpressionType>
template<typename OtherDerived>
inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::equal_to)
Cwise<ExpressionType>::operator==(const MatrixBase<OtherDerived> &other) const
{
return EIGEN_CWISE_BINOP_RETURN_TYPE(std::equal_to)(_expression(), other.derived());
}
/** \array_module
*
* \returns an expression of the coefficient-wise != operator of *this and \a other
*
* \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
* In order to check for equality between two vectors or matrices with floating-point coefficients, it is
* generally a far better idea to use a fuzzy comparison as provided by MatrixBase::isApprox() and
* MatrixBase::isMuchSmallerThan().
*
* Example: \include Cwise_not_equal.cpp
* Output: \verbinclude Cwise_not_equal.out
*
* \sa MatrixBase::all(), MatrixBase::any(), MatrixBase::isApprox(), MatrixBase::isMuchSmallerThan()
*/
template<typename ExpressionType>
template<typename OtherDerived>
inline const EIGEN_CWISE_BINOP_RETURN_TYPE(std::not_equal_to)
Cwise<ExpressionType>::operator!=(const MatrixBase<OtherDerived> &other) const
{
return EIGEN_CWISE_BINOP_RETURN_TYPE(std::not_equal_to)(_expression(), other.derived());
}
// comparisons to scalar value
/** \array_module
*
* \returns an expression of the coefficient-wise \< operator of *this and a scalar \a s
*
* \sa operator<(const MatrixBase<OtherDerived> &) const
*/
template<typename ExpressionType>
inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less)
Cwise<ExpressionType>::operator<(Scalar s) const
{
return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less)(_expression(),
typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s));
}
/** \array_module
*
* \returns an expression of the coefficient-wise \<= operator of *this and a scalar \a s
*
* \sa operator<=(const MatrixBase<OtherDerived> &) const
*/
template<typename ExpressionType>
inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less_equal)
Cwise<ExpressionType>::operator<=(Scalar s) const
{
return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less_equal)(_expression(),
typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s));
}
/** \array_module
*
* \returns an expression of the coefficient-wise \> operator of *this and a scalar \a s
*
* \sa operator>(const MatrixBase<OtherDerived> &) const
*/
template<typename ExpressionType>
inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater)
Cwise<ExpressionType>::operator>(Scalar s) const
{
return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater)(_expression(),
typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s));
}
/** \array_module
*
* \returns an expression of the coefficient-wise \>= operator of *this and a scalar \a s
*
* \sa operator>=(const MatrixBase<OtherDerived> &) const
*/
template<typename ExpressionType>
inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater_equal)
Cwise<ExpressionType>::operator>=(Scalar s) const
{
return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater_equal)(_expression(),
typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s));
}
/** \array_module
*
* \returns an expression of the coefficient-wise == operator of *this and a scalar \a s
*
* \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
* In order to check for equality between two vectors or matrices with floating-point coefficients, it is
* generally a far better idea to use a fuzzy comparison as provided by MatrixBase::isApprox() and
* MatrixBase::isMuchSmallerThan().
*
* \sa operator==(const MatrixBase<OtherDerived> &) const
*/
template<typename ExpressionType>
inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::equal_to)
Cwise<ExpressionType>::operator==(Scalar s) const
{
return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::equal_to)(_expression(),
typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s));
}
/** \array_module
*
* \returns an expression of the coefficient-wise != operator of *this and a scalar \a s
*
* \warning this performs an exact comparison, which is generally a bad idea with floating-point types.
* In order to check for equality between two vectors or matrices with floating-point coefficients, it is
* generally a far better idea to use a fuzzy comparison as provided by MatrixBase::isApprox() and
* MatrixBase::isMuchSmallerThan().
*
* \sa operator!=(const MatrixBase<OtherDerived> &) const
*/
template<typename ExpressionType>
inline const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::not_equal_to)
Cwise<ExpressionType>::operator!=(Scalar s) const
{
return EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::not_equal_to)(_expression(),
typename ExpressionType::ConstantReturnType(_expression().rows(), _expression().cols(), s));
}
// scalar addition
/** \array_module
*
* \returns an expression of \c *this with each coeff incremented by the constant \a scalar
*
* Example: \include Cwise_plus.cpp
* Output: \verbinclude Cwise_plus.out
*
* \sa operator+=(), operator-()
*/
template<typename ExpressionType>
inline const typename Cwise<ExpressionType>::ScalarAddReturnType
Cwise<ExpressionType>::operator+(const Scalar& scalar) const
{
return typename Cwise<ExpressionType>::ScalarAddReturnType(m_matrix, ei_scalar_add_op<Scalar>(scalar));
}
/** \array_module
*
* Adds the given \a scalar to each coeff of this expression.
*
* Example: \include Cwise_plus_equal.cpp
* Output: \verbinclude Cwise_plus_equal.out
*
* \sa operator+(), operator-=()
*/
template<typename ExpressionType>
inline ExpressionType& Cwise<ExpressionType>::operator+=(const Scalar& scalar)
{
return m_matrix.const_cast_derived() = *this + scalar;
}
/** \array_module
*
* \returns an expression of \c *this with each coeff decremented by the constant \a scalar
*
* Example: \include Cwise_minus.cpp
* Output: \verbinclude Cwise_minus.out
*
* \sa operator+(), operator-=()
*/
template<typename ExpressionType>
inline const typename Cwise<ExpressionType>::ScalarAddReturnType
Cwise<ExpressionType>::operator-(const Scalar& scalar) const
{
return *this + (-scalar);
}
/** \array_module
*
* Substracts the given \a scalar from each coeff of this expression.
*
* Example: \include Cwise_minus_equal.cpp
* Output: \verbinclude Cwise_minus_equal.out
*
* \sa operator+=(), operator-()
*/
template<typename ExpressionType>
inline ExpressionType& Cwise<ExpressionType>::operator-=(const Scalar& scalar)
{
return m_matrix.const_cast_derived() = *this - scalar;
}
#endif // EIGEN_ARRAY_CWISE_OPERATORS_H

309
ground/openpilotgcs/src/libs/eigen/Eigen/src/Array/Functors.h Normal file
View File

@ -0,0 +1,309 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_ARRAY_FUNCTORS_H
#define EIGEN_ARRAY_FUNCTORS_H
/** \internal
* \array_module
*
* \brief Template functor to add a scalar to a fixed other one
*
* \sa class CwiseUnaryOp, Array::operator+
*/
/* If you wonder why doing the ei_pset1() in packetOp() is an optimization check ei_scalar_multiple_op */
template<typename Scalar>
struct ei_scalar_add_op {
typedef typename ei_packet_traits<Scalar>::type PacketScalar;
// FIXME default copy constructors seems bugged with std::complex<>
inline ei_scalar_add_op(const ei_scalar_add_op& other) : m_other(other.m_other) { }
inline ei_scalar_add_op(const Scalar& other) : m_other(other) { }
inline Scalar operator() (const Scalar& a) const { return a + m_other; }
inline const PacketScalar packetOp(const PacketScalar& a) const
{ return ei_padd(a, ei_pset1(m_other)); }
const Scalar m_other;
private:
ei_scalar_add_op& operator=(const ei_scalar_add_op&);
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_add_op<Scalar> >
{ enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = ei_packet_traits<Scalar>::size>1 }; };
/** \internal
*
* \array_module
*
* \brief Template functor to compute the square root of a scalar
*
* \sa class CwiseUnaryOp, Cwise::sqrt()
*/
template<typename Scalar> struct ei_scalar_sqrt_op EIGEN_EMPTY_STRUCT {
inline const Scalar operator() (const Scalar& a) const { return ei_sqrt(a); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_sqrt_op<Scalar> >
{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false }; };
/** \internal
*
* \array_module
*
* \brief Template functor to compute the exponential of a scalar
*
* \sa class CwiseUnaryOp, Cwise::exp()
*/
template<typename Scalar> struct ei_scalar_exp_op EIGEN_EMPTY_STRUCT {
inline const Scalar operator() (const Scalar& a) const { return ei_exp(a); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_exp_op<Scalar> >
{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false }; };
/** \internal
*
* \array_module
*
* \brief Template functor to compute the logarithm of a scalar
*
* \sa class CwiseUnaryOp, Cwise::log()
*/
template<typename Scalar> struct ei_scalar_log_op EIGEN_EMPTY_STRUCT {
inline const Scalar operator() (const Scalar& a) const { return ei_log(a); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_log_op<Scalar> >
{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false }; };
/** \internal
*
* \array_module
*
* \brief Template functor to compute the cosine of a scalar
*
* \sa class CwiseUnaryOp, Cwise::cos()
*/
template<typename Scalar> struct ei_scalar_cos_op EIGEN_EMPTY_STRUCT {
inline const Scalar operator() (const Scalar& a) const { return ei_cos(a); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_cos_op<Scalar> >
{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false }; };
/** \internal
*
* \array_module
*
* \brief Template functor to compute the sine of a scalar
*
* \sa class CwiseUnaryOp, Cwise::sin()
*/
template<typename Scalar> struct ei_scalar_sin_op EIGEN_EMPTY_STRUCT {
inline const Scalar operator() (const Scalar& a) const { return ei_sin(a); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_sin_op<Scalar> >
{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false }; };
/** \internal
*
* \array_module
*
* \brief Template functor to raise a scalar to a power
*
* \sa class CwiseUnaryOp, Cwise::pow
*/
template<typename Scalar>
struct ei_scalar_pow_op {
// FIXME default copy constructors seems bugged with std::complex<>
inline ei_scalar_pow_op(const ei_scalar_pow_op& other) : m_exponent(other.m_exponent) { }
inline ei_scalar_pow_op(const Scalar& exponent) : m_exponent(exponent) {}
inline Scalar operator() (const Scalar& a) const { return ei_pow(a, m_exponent); }
const Scalar m_exponent;
private:
ei_scalar_pow_op& operator=(const ei_scalar_pow_op&);
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_pow_op<Scalar> >
{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false }; };
/** \internal
*
* \array_module
*
* \brief Template functor to compute the inverse of a scalar
*
* \sa class CwiseUnaryOp, Cwise::inverse()
*/
template<typename Scalar>
struct ei_scalar_inverse_op {
inline Scalar operator() (const Scalar& a) const { return Scalar(1)/a; }
template<typename PacketScalar>
inline const PacketScalar packetOp(const PacketScalar& a) const
{ return ei_pdiv(ei_pset1(Scalar(1)),a); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_inverse_op<Scalar> >
{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = int(ei_packet_traits<Scalar>::size)>1 }; };
/** \internal
*
* \array_module
*
* \brief Template functor to compute the square of a scalar
*
* \sa class CwiseUnaryOp, Cwise::square()
*/
template<typename Scalar>
struct ei_scalar_square_op {
inline Scalar operator() (const Scalar& a) const { return a*a; }
template<typename PacketScalar>
inline const PacketScalar packetOp(const PacketScalar& a) const
{ return ei_pmul(a,a); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_square_op<Scalar> >
{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = int(ei_packet_traits<Scalar>::size)>1 }; };
/** \internal
*
* \array_module
*
* \brief Template functor to compute the cube of a scalar
*
* \sa class CwiseUnaryOp, Cwise::cube()
*/
template<typename Scalar>
struct ei_scalar_cube_op {
inline Scalar operator() (const Scalar& a) const { return a*a*a; }
template<typename PacketScalar>
inline const PacketScalar packetOp(const PacketScalar& a) const
{ return ei_pmul(a,ei_pmul(a,a)); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_cube_op<Scalar> >
{ enum { Cost = 2*NumTraits<Scalar>::MulCost, PacketAccess = int(ei_packet_traits<Scalar>::size)>1 }; };
// default ei_functor_traits for STL functors:
template<typename T>
struct ei_functor_traits<std::multiplies<T> >
{ enum { Cost = NumTraits<T>::MulCost, PacketAccess = false }; };
template<typename T>
struct ei_functor_traits<std::divides<T> >
{ enum { Cost = NumTraits<T>::MulCost, PacketAccess = false }; };
template<typename T>
struct ei_functor_traits<std::plus<T> >
{ enum { Cost = NumTraits<T>::AddCost, PacketAccess = false }; };
template<typename T>
struct ei_functor_traits<std::minus<T> >
{ enum { Cost = NumTraits<T>::AddCost, PacketAccess = false }; };
template<typename T>
struct ei_functor_traits<std::negate<T> >
{ enum { Cost = NumTraits<T>::AddCost, PacketAccess = false }; };
template<typename T>
struct ei_functor_traits<std::logical_or<T> >
{ enum { Cost = 1, PacketAccess = false }; };
template<typename T>
struct ei_functor_traits<std::logical_and<T> >
{ enum { Cost = 1, PacketAccess = false }; };
template<typename T>
struct ei_functor_traits<std::logical_not<T> >
{ enum { Cost = 1, PacketAccess = false }; };
template<typename T>
struct ei_functor_traits<std::greater<T> >
{ enum { Cost = 1, PacketAccess = false }; };
template<typename T>
struct ei_functor_traits<std::less<T> >
{ enum { Cost = 1, PacketAccess = false }; };
template<typename T>
struct ei_functor_traits<std::greater_equal<T> >
{ enum { Cost = 1, PacketAccess = false }; };
template<typename T>
struct ei_functor_traits<std::less_equal<T> >
{ enum { Cost = 1, PacketAccess = false }; };
template<typename T>
struct ei_functor_traits<std::equal_to<T> >
{ enum { Cost = 1, PacketAccess = false }; };
template<typename T>
struct ei_functor_traits<std::not_equal_to<T> >
{ enum { Cost = 1, PacketAccess = false }; };
template<typename T>
struct ei_functor_traits<std::binder2nd<T> >
{ enum { Cost = ei_functor_traits<T>::Cost, PacketAccess = false }; };
template<typename T>
struct ei_functor_traits<std::binder1st<T> >
{ enum { Cost = ei_functor_traits<T>::Cost, PacketAccess = false }; };
template<typename T>
struct ei_functor_traits<std::unary_negate<T> >
{ enum { Cost = 1 + ei_functor_traits<T>::Cost, PacketAccess = false }; };
template<typename T>
struct ei_functor_traits<std::binary_negate<T> >
{ enum { Cost = 1 + ei_functor_traits<T>::Cost, PacketAccess = false }; };
#ifdef EIGEN_STDEXT_SUPPORT
template<typename T0,typename T1>
struct ei_functor_traits<std::project1st<T0,T1> >
{ enum { Cost = 0, PacketAccess = false }; };
template<typename T0,typename T1>
struct ei_functor_traits<std::project2nd<T0,T1> >
{ enum { Cost = 0, PacketAccess = false }; };
template<typename T0,typename T1>
struct ei_functor_traits<std::select2nd<std::pair<T0,T1> > >
{ enum { Cost = 0, PacketAccess = false }; };
template<typename T0,typename T1>
struct ei_functor_traits<std::select1st<std::pair<T0,T1> > >
{ enum { Cost = 0, PacketAccess = false }; };
template<typename T0,typename T1>
struct ei_functor_traits<std::unary_compose<T0,T1> >
{ enum { Cost = ei_functor_traits<T0>::Cost + ei_functor_traits<T1>::Cost, PacketAccess = false }; };
template<typename T0,typename T1,typename T2>
struct ei_functor_traits<std::binary_compose<T0,T1,T2> >
{ enum { Cost = ei_functor_traits<T0>::Cost + ei_functor_traits<T1>::Cost + ei_functor_traits<T2>::Cost, PacketAccess = false }; };
#endif // EIGEN_STDEXT_SUPPORT
#endif // EIGEN_ARRAY_FUNCTORS_H

80
ground/openpilotgcs/src/libs/eigen/Eigen/src/Array/Norms.h Normal file
View File

@ -0,0 +1,80 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_ARRAY_NORMS_H
#define EIGEN_ARRAY_NORMS_H
template<typename Derived, int p>
struct ei_lpNorm_selector
{
typedef typename NumTraits<typename ei_traits<Derived>::Scalar>::Real RealScalar;
inline static RealScalar run(const MatrixBase<Derived>& m)
{
return ei_pow(m.cwise().abs().cwise().pow(p).sum(), RealScalar(1)/p);
}
};
template<typename Derived>
struct ei_lpNorm_selector<Derived, 1>
{
inline static typename NumTraits<typename ei_traits<Derived>::Scalar>::Real run(const MatrixBase<Derived>& m)
{
return m.cwise().abs().sum();
}
};
template<typename Derived>
struct ei_lpNorm_selector<Derived, 2>
{
inline static typename NumTraits<typename ei_traits<Derived>::Scalar>::Real run(const MatrixBase<Derived>& m)
{
return m.norm();
}
};
template<typename Derived>
struct ei_lpNorm_selector<Derived, Infinity>
{
inline static typename NumTraits<typename ei_traits<Derived>::Scalar>::Real run(const MatrixBase<Derived>& m)
{
return m.cwise().abs().maxCoeff();
}
};
/** \array_module
*
* \returns the \f$ \ell^p \f$ norm of *this, that is, returns the p-th root of the sum of the p-th powers of the absolute values
* of the coefficients of *this. If \a p is the special value \a Eigen::Infinity, this function returns the \f$ \ell^p\infty \f$
* norm, that is the maximum of the absolute values of the coefficients of *this.
*
* \sa norm()
*/
template<typename Derived>
template<int p>
inline typename NumTraits<typename ei_traits<Derived>::Scalar>::Real MatrixBase<Derived>::lpNorm() const
{
return ei_lpNorm_selector<Derived, p>::run(*this);
}
#endif // EIGEN_ARRAY_NORMS_H

349
ground/openpilotgcs/src/libs/eigen/Eigen/src/Array/PartialRedux.h Normal file
View File

@ -0,0 +1,349 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_PARTIAL_REDUX_H
#define EIGEN_PARTIAL_REDUX_H
/** \array_module \ingroup Array
*
* \class PartialReduxExpr
*
* \brief Generic expression of a partially reduxed matrix
*
* \param MatrixType the type of the matrix we are applying the redux operation
* \param MemberOp type of the member functor
* \param Direction indicates the direction of the redux (Vertical or Horizontal)
*
* This class represents an expression of a partial redux operator of a matrix.
* It is the return type of PartialRedux functions,
* and most of the time this is the only way it is used.
*
* \sa class PartialRedux
*/
template< typename MatrixType, typename MemberOp, int Direction>
class PartialReduxExpr;
template<typename MatrixType, typename MemberOp, int Direction>
struct ei_traits<PartialReduxExpr<MatrixType, MemberOp, Direction> >
{
typedef typename MemberOp::result_type Scalar;
typedef typename MatrixType::Scalar InputScalar;
typedef typename ei_nested<MatrixType>::type MatrixTypeNested;
typedef typename ei_cleantype<MatrixTypeNested>::type _MatrixTypeNested;
enum {
RowsAtCompileTime = Direction==Vertical ? 1 : MatrixType::RowsAtCompileTime,
ColsAtCompileTime = Direction==Horizontal ? 1 : MatrixType::ColsAtCompileTime,
MaxRowsAtCompileTime = Direction==Vertical ? 1 : MatrixType::MaxRowsAtCompileTime,
MaxColsAtCompileTime = Direction==Horizontal ? 1 : MatrixType::MaxColsAtCompileTime,
Flags = (unsigned int)_MatrixTypeNested::Flags & HereditaryBits,
TraversalSize = Direction==Vertical ? RowsAtCompileTime : ColsAtCompileTime
};
#if EIGEN_GNUC_AT_LEAST(3,4)
typedef typename MemberOp::template Cost<InputScalar,int(TraversalSize)> CostOpType;
#else
typedef typename MemberOp::template Cost<InputScalar,TraversalSize> CostOpType;
#endif
enum {
CoeffReadCost = TraversalSize * ei_traits<_MatrixTypeNested>::CoeffReadCost + int(CostOpType::value)
};
};
template< typename MatrixType, typename MemberOp, int Direction>
class PartialReduxExpr : ei_no_assignment_operator,
public MatrixBase<PartialReduxExpr<MatrixType, MemberOp, Direction> >
{
public:
EIGEN_GENERIC_PUBLIC_INTERFACE(PartialReduxExpr)
typedef typename ei_traits<PartialReduxExpr>::MatrixTypeNested MatrixTypeNested;
typedef typename ei_traits<PartialReduxExpr>::_MatrixTypeNested _MatrixTypeNested;
PartialReduxExpr(const MatrixType& mat, const MemberOp& func = MemberOp())
: m_matrix(mat), m_functor(func) {}
int rows() const { return (Direction==Vertical ? 1 : m_matrix.rows()); }
int cols() const { return (Direction==Horizontal ? 1 : m_matrix.cols()); }
const Scalar coeff(int i, int j) const
{
if (Direction==Vertical)
return m_functor(m_matrix.col(j));
else
return m_functor(m_matrix.row(i));
}
protected:
const MatrixTypeNested m_matrix;
const MemberOp m_functor;
};
#define EIGEN_MEMBER_FUNCTOR(MEMBER,COST) \
template <typename ResultType> \
struct ei_member_##MEMBER EIGEN_EMPTY_STRUCT { \
typedef ResultType result_type; \
template<typename Scalar, int Size> struct Cost \
{ enum { value = COST }; }; \
template<typename Derived> \
inline ResultType operator()(const MatrixBase<Derived>& mat) const \
{ return mat.MEMBER(); } \
}
EIGEN_MEMBER_FUNCTOR(squaredNorm, Size * NumTraits<Scalar>::MulCost + (Size-1)*NumTraits<Scalar>::AddCost);
EIGEN_MEMBER_FUNCTOR(norm, (Size+5) * NumTraits<Scalar>::MulCost + (Size-1)*NumTraits<Scalar>::AddCost);
EIGEN_MEMBER_FUNCTOR(sum, (Size-1)*NumTraits<Scalar>::AddCost);
EIGEN_MEMBER_FUNCTOR(minCoeff, (Size-1)*NumTraits<Scalar>::AddCost);
EIGEN_MEMBER_FUNCTOR(maxCoeff, (Size-1)*NumTraits<Scalar>::AddCost);
EIGEN_MEMBER_FUNCTOR(all, (Size-1)*NumTraits<Scalar>::AddCost);
EIGEN_MEMBER_FUNCTOR(any, (Size-1)*NumTraits<Scalar>::AddCost);
EIGEN_MEMBER_FUNCTOR(count, (Size-1)*NumTraits<Scalar>::AddCost);
/** \internal */
template <typename BinaryOp, typename Scalar>
struct ei_member_redux {
typedef typename ei_result_of<
BinaryOp(Scalar)
>::type result_type;
template<typename _Scalar, int Size> struct Cost
{ enum { value = (Size-1) * ei_functor_traits<BinaryOp>::Cost }; };
ei_member_redux(const BinaryOp func) : m_functor(func) {}
template<typename Derived>
inline result_type operator()(const MatrixBase<Derived>& mat) const
{ return mat.redux(m_functor); }
const BinaryOp m_functor;
private:
ei_member_redux& operator=(const ei_member_redux&);
};
/** \array_module \ingroup Array
*
* \class PartialRedux
*
* \brief Pseudo expression providing partial reduction operations
*
* \param ExpressionType the type of the object on which to do partial reductions
* \param Direction indicates the direction of the redux (Vertical or Horizontal)
*
* This class represents a pseudo expression with partial reduction features.
* It is the return type of MatrixBase::colwise() and MatrixBase::rowwise()
* and most of the time this is the only way it is used.
*
* Example: \include MatrixBase_colwise.cpp
* Output: \verbinclude MatrixBase_colwise.out
*
* \sa MatrixBase::colwise(), MatrixBase::rowwise(), class PartialReduxExpr
*/
template<typename ExpressionType, int Direction> class PartialRedux
{
public:
typedef typename ei_traits<ExpressionType>::Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
typedef typename ei_meta_if<ei_must_nest_by_value<ExpressionType>::ret,
ExpressionType, const ExpressionType&>::ret ExpressionTypeNested;
template<template<typename _Scalar> class Functor,
typename Scalar = typename ei_traits<ExpressionType>::Scalar> struct ReturnType
{
typedef PartialReduxExpr<ExpressionType,
Functor<Scalar>,
Direction
> Type;
};
template<typename BinaryOp> struct ReduxReturnType
{
typedef PartialReduxExpr<ExpressionType,
ei_member_redux<BinaryOp,typename ei_traits<ExpressionType>::Scalar>,
Direction
> Type;
};
typedef typename ExpressionType::PlainMatrixType CrossReturnType;
inline PartialRedux(const ExpressionType& matrix) : m_matrix(matrix) {}
/** \internal */
inline const ExpressionType& _expression() const { return m_matrix; }
template<typename BinaryOp>
const typename ReduxReturnType<BinaryOp>::Type
redux(const BinaryOp& func = BinaryOp()) const;
/** \returns a row (or column) vector expression of the smallest coefficient
* of each column (or row) of the referenced expression.
*
* Example: \include PartialRedux_minCoeff.cpp
* Output: \verbinclude PartialRedux_minCoeff.out
*
* \sa MatrixBase::minCoeff() */
const typename ReturnType<ei_member_minCoeff>::Type minCoeff() const
{ return _expression(); }
/** \returns a row (or column) vector expression of the largest coefficient
* of each column (or row) of the referenced expression.
*
* Example: \include PartialRedux_maxCoeff.cpp
* Output: \verbinclude PartialRedux_maxCoeff.out
*
* \sa MatrixBase::maxCoeff() */
const typename ReturnType<ei_member_maxCoeff>::Type maxCoeff() const
{ return _expression(); }
/** \returns a row (or column) vector expression of the squared norm
* of each column (or row) of the referenced expression.
*
* Example: \include PartialRedux_squaredNorm.cpp
* Output: \verbinclude PartialRedux_squaredNorm.out
*
* \sa MatrixBase::squaredNorm() */
const typename ReturnType<ei_member_squaredNorm,RealScalar>::Type squaredNorm() const
{ return _expression(); }
/** \returns a row (or column) vector expression of the norm
* of each column (or row) of the referenced expression.
*
* Example: \include PartialRedux_norm.cpp
* Output: \verbinclude PartialRedux_norm.out
*
* \sa MatrixBase::norm() */
const typename ReturnType<ei_member_norm,RealScalar>::Type norm() const
{ return _expression(); }
/** \returns a row (or column) vector expression of the sum
* of each column (or row) of the referenced expression.
*
* Example: \include PartialRedux_sum.cpp
* Output: \verbinclude PartialRedux_sum.out
*
* \sa MatrixBase::sum() */
const typename ReturnType<ei_member_sum>::Type sum() const
{ return _expression(); }
/** \returns a row (or column) vector expression representing
* whether \b all coefficients of each respective column (or row) are \c true.
*
* \sa MatrixBase::all() */
const typename ReturnType<ei_member_all>::Type all() const
{ return _expression(); }
/** \returns a row (or column) vector expression representing
* whether \b at \b least one coefficient of each respective column (or row) is \c true.
*
* \sa MatrixBase::any() */
const typename ReturnType<ei_member_any>::Type any() const
{ return _expression(); }
/** \returns a row (or column) vector expression representing
* the number of \c true coefficients of each respective column (or row).
*
* Example: \include PartialRedux_count.cpp
* Output: \verbinclude PartialRedux_count.out
*
* \sa MatrixBase::count() */
const PartialReduxExpr<ExpressionType, ei_member_count<int>, Direction> count() const
{ return _expression(); }
/** \returns a 3x3 matrix expression of the cross product
* of each column or row of the referenced expression with the \a other vector.
*
* \geometry_module
*
* \sa MatrixBase::cross() */
template<typename OtherDerived>
const CrossReturnType cross(const MatrixBase<OtherDerived>& other) const
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(CrossReturnType,3,3)
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,3)
EIGEN_STATIC_ASSERT((ei_is_same_type<Scalar, typename OtherDerived::Scalar>::ret),
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
if(Direction==Vertical)
return (CrossReturnType()
<< _expression().col(0).cross(other),
_expression().col(1).cross(other),
_expression().col(2).cross(other)).finished();
else
return (CrossReturnType()
<< _expression().row(0).cross(other),
_expression().row(1).cross(other),
_expression().row(2).cross(other)).finished();
}
protected:
ExpressionTypeNested m_matrix;
private:
PartialRedux& operator=(const PartialRedux&);
};
/** \array_module
*
* \returns a PartialRedux wrapper of *this providing additional partial reduction operations
*
* Example: \include MatrixBase_colwise.cpp
* Output: \verbinclude MatrixBase_colwise.out
*
* \sa rowwise(), class PartialRedux
*/
template<typename Derived>
inline const PartialRedux<Derived,Vertical>
MatrixBase<Derived>::colwise() const
{
return derived();
}
/** \array_module
*
* \returns a PartialRedux wrapper of *this providing additional partial reduction operations
*
* Example: \include MatrixBase_rowwise.cpp
* Output: \verbinclude MatrixBase_rowwise.out
*
* \sa colwise(), class PartialRedux
*/
template<typename Derived>
inline const PartialRedux<Derived,Horizontal>
MatrixBase<Derived>::rowwise() const
{
return derived();
}
/** \returns a row or column vector expression of \c *this reduxed by \a func
*
* The template parameter \a BinaryOp is the type of the functor
* of the custom redux operator. Note that func must be an associative operator.
*
* \sa class PartialRedux, MatrixBase::colwise(), MatrixBase::rowwise()
*/
template<typename ExpressionType, int Direction>
template<typename BinaryOp>
const typename PartialRedux<ExpressionType,Direction>::template ReduxReturnType<BinaryOp>::Type
PartialRedux<ExpressionType,Direction>::redux(const BinaryOp& func) const
{
return typename ReduxReturnType<BinaryOp>::Type(_expression(), func);
}
#endif // EIGEN_PARTIAL_REDUX_H

156
ground/openpilotgcs/src/libs/eigen/Eigen/src/Array/Random.h Normal file
View File

@ -0,0 +1,156 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_RANDOM_H
#define EIGEN_RANDOM_H
template<typename Scalar> struct ei_scalar_random_op EIGEN_EMPTY_STRUCT {
inline ei_scalar_random_op(void) {}
inline const Scalar operator() (int, int) const { return ei_random<Scalar>(); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_random_op<Scalar> >
{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false, IsRepeatable = false }; };
/** \array_module
*
* \returns a random matrix (not an expression, the matrix is immediately evaluated).
*
* The parameters \a rows and \a cols are the number of rows and of columns of
* the returned matrix. Must be compatible with this MatrixBase type.
*
* This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
* it is redundant to pass \a rows and \a cols as arguments, so ei_random() should be used
* instead.
*
* \addexample RandomExample \label How to create a matrix with random coefficients
*
* Example: \include MatrixBase_random_int_int.cpp
* Output: \verbinclude MatrixBase_random_int_int.out
*
* \sa MatrixBase::setRandom(), MatrixBase::Random(int), MatrixBase::Random()
*/
template<typename Derived>
inline const CwiseNullaryOp<ei_scalar_random_op<typename ei_traits<Derived>::Scalar>, Derived>
MatrixBase<Derived>::Random(int rows, int cols)
{
return NullaryExpr(rows, cols, ei_scalar_random_op<Scalar>());
}
/** \array_module
*
* \returns a random vector (not an expression, the vector is immediately evaluated).
*
* The parameter \a size is the size of the returned vector.
* Must be compatible with this MatrixBase type.
*
* \only_for_vectors
*
* This variant is meant to be used for dynamic-size vector types. For fixed-size types,
* it is redundant to pass \a size as argument, so ei_random() should be used
* instead.
*
* Example: \include MatrixBase_random_int.cpp
* Output: \verbinclude MatrixBase_random_int.out
*
* \sa MatrixBase::setRandom(), MatrixBase::Random(int,int), MatrixBase::Random()
*/
template<typename Derived>
inline const CwiseNullaryOp<ei_scalar_random_op<typename ei_traits<Derived>::Scalar>, Derived>
MatrixBase<Derived>::Random(int size)
{
return NullaryExpr(size, ei_scalar_random_op<Scalar>());
}
/** \array_module
*
* \returns a fixed-size random matrix or vector
* (not an expression, the matrix is immediately evaluated).
*
* This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
* need to use the variants taking size arguments.
*
* Example: \include MatrixBase_random.cpp
* Output: \verbinclude MatrixBase_random.out
*
* \sa MatrixBase::setRandom(), MatrixBase::Random(int,int), MatrixBase::Random(int)
*/
template<typename Derived>
inline const CwiseNullaryOp<ei_scalar_random_op<typename ei_traits<Derived>::Scalar>, Derived>
MatrixBase<Derived>::Random()
{
return NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, ei_scalar_random_op<Scalar>());
}
/** \array_module
*
* Sets all coefficients in this expression to random values.
*
* Example: \include MatrixBase_setRandom.cpp
* Output: \verbinclude MatrixBase_setRandom.out
*
* \sa class CwiseNullaryOp, setRandom(int), setRandom(int,int)
*/
template<typename Derived>
inline Derived& MatrixBase<Derived>::setRandom()
{
return *this = Random(rows(), cols());
}
/** Resizes to the given \a size, and sets all coefficients in this expression to random values.
*
* \only_for_vectors
*
* Example: \include Matrix_setRandom_int.cpp
* Output: \verbinclude Matrix_setRandom_int.out
*
* \sa MatrixBase::setRandom(), setRandom(int,int), class CwiseNullaryOp, MatrixBase::Random()
*/
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setRandom(int size)
{
resize(size);
return setRandom();
}
/** Resizes to the given size, and sets all coefficients in this expression to random values.
*
* \param rows the new number of rows
* \param cols the new number of columns
*
* Example: \include Matrix_setRandom_int_int.cpp
* Output: \verbinclude Matrix_setRandom_int_int.out
*
* \sa MatrixBase::setRandom(), setRandom(int), class CwiseNullaryOp, MatrixBase::Random()
*/
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setRandom(int rows, int cols)
{
resize(rows, cols);
return setRandom();
}
#endif // EIGEN_RANDOM_H

159
ground/openpilotgcs/src/libs/eigen/Eigen/src/Array/Select.h Normal file
View File

@ -0,0 +1,159 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_SELECT_H
#define EIGEN_SELECT_H
/** \array_module \ingroup Array
*
* \class Select
*
* \brief Expression of a coefficient wise version of the C++ ternary operator ?:
*
* \param ConditionMatrixType the type of the \em condition expression which must be a boolean matrix
* \param ThenMatrixType the type of the \em then expression
* \param ElseMatrixType the type of the \em else expression
*
* This class represents an expression of a coefficient wise version of the C++ ternary operator ?:.
* It is the return type of MatrixBase::select() and most of the time this is the only way it is used.
*
* \sa MatrixBase::select(const MatrixBase<ThenDerived>&, const MatrixBase<ElseDerived>&) const
*/
template<typename ConditionMatrixType, typename ThenMatrixType, typename ElseMatrixType>
struct ei_traits<Select<ConditionMatrixType, ThenMatrixType, ElseMatrixType> >
{
typedef typename ei_traits<ThenMatrixType>::Scalar Scalar;
typedef typename ConditionMatrixType::Nested ConditionMatrixNested;
typedef typename ThenMatrixType::Nested ThenMatrixNested;
typedef typename ElseMatrixType::Nested ElseMatrixNested;
enum {
RowsAtCompileTime = ConditionMatrixType::RowsAtCompileTime,
ColsAtCompileTime = ConditionMatrixType::ColsAtCompileTime,
MaxRowsAtCompileTime = ConditionMatrixType::MaxRowsAtCompileTime,
MaxColsAtCompileTime = ConditionMatrixType::MaxColsAtCompileTime,
Flags = (unsigned int)ThenMatrixType::Flags & ElseMatrixType::Flags & HereditaryBits,
CoeffReadCost = ei_traits<typename ei_cleantype<ConditionMatrixNested>::type>::CoeffReadCost
+ EIGEN_ENUM_MAX(ei_traits<typename ei_cleantype<ThenMatrixNested>::type>::CoeffReadCost,
ei_traits<typename ei_cleantype<ElseMatrixNested>::type>::CoeffReadCost)
};
};
template<typename ConditionMatrixType, typename ThenMatrixType, typename ElseMatrixType>
class Select : ei_no_assignment_operator,
public MatrixBase<Select<ConditionMatrixType, ThenMatrixType, ElseMatrixType> >
{
public:
EIGEN_GENERIC_PUBLIC_INTERFACE(Select)
Select(const ConditionMatrixType& conditionMatrix,
const ThenMatrixType& thenMatrix,
const ElseMatrixType& elseMatrix)
: m_condition(conditionMatrix), m_then(thenMatrix), m_else(elseMatrix)
{
ei_assert(m_condition.rows() == m_then.rows() && m_condition.rows() == m_else.rows());
ei_assert(m_condition.cols() == m_then.cols() && m_condition.cols() == m_else.cols());
}
int rows() const { return m_condition.rows(); }
int cols() const { return m_condition.cols(); }
const Scalar coeff(int i, int j) const
{
if (m_condition.coeff(i,j))
return m_then.coeff(i,j);
else
return m_else.coeff(i,j);
}
const Scalar coeff(int i) const
{
if (m_condition.coeff(i))
return m_then.coeff(i);
else
return m_else.coeff(i);
}
protected:
const typename ConditionMatrixType::Nested m_condition;
const typename ThenMatrixType::Nested m_then;
const typename ElseMatrixType::Nested m_else;
};
/** \array_module
*
* \returns a matrix where each coefficient (i,j) is equal to \a thenMatrix(i,j)
* if \c *this(i,j), and \a elseMatrix(i,j) otherwise.
*
* Example: \include MatrixBase_select.cpp
* Output: \verbinclude MatrixBase_select.out
*
* \sa class Select
*/
template<typename Derived>
template<typename ThenDerived,typename ElseDerived>
inline const Select<Derived,ThenDerived,ElseDerived>
MatrixBase<Derived>::select(const MatrixBase<ThenDerived>& thenMatrix,
const MatrixBase<ElseDerived>& elseMatrix) const
{
return Select<Derived,ThenDerived,ElseDerived>(derived(), thenMatrix.derived(), elseMatrix.derived());
}
/** \array_module
*
* Version of MatrixBase::select(const MatrixBase&, const MatrixBase&) with
* the \em else expression being a scalar value.
*
* \sa MatrixBase::select(const MatrixBase<ThenDerived>&, const MatrixBase<ElseDerived>&) const, class Select
*/
template<typename Derived>
template<typename ThenDerived>
inline const Select<Derived,ThenDerived, NestByValue<typename ThenDerived::ConstantReturnType> >
MatrixBase<Derived>::select(const MatrixBase<ThenDerived>& thenMatrix,
typename ThenDerived::Scalar elseScalar) const
{
return Select<Derived,ThenDerived,NestByValue<typename ThenDerived::ConstantReturnType> >(
derived(), thenMatrix.derived(), ThenDerived::Constant(rows(),cols(),elseScalar));
}
/** \array_module
*
* Version of MatrixBase::select(const MatrixBase&, const MatrixBase&) with
* the \em then expression being a scalar value.
*
* \sa MatrixBase::select(const MatrixBase<ThenDerived>&, const MatrixBase<ElseDerived>&) const, class Select
*/
template<typename Derived>
template<typename ElseDerived>
inline const Select<Derived, NestByValue<typename ElseDerived::ConstantReturnType>, ElseDerived >
MatrixBase<Derived>::select(typename ElseDerived::Scalar thenScalar,
const MatrixBase<ElseDerived>& elseMatrix) const
{
return Select<Derived,NestByValue<typename ElseDerived::ConstantReturnType>,ElseDerived>(
derived(), ElseDerived::Constant(rows(),cols(),thenScalar), elseMatrix.derived());
}
#endif // EIGEN_SELECT_H

6
ground/openpilotgcs/src/libs/eigen/Eigen/src/Cholesky/CMakeLists.txt Normal file
View File

@ -0,0 +1,6 @@
FILE(GLOB Eigen_Cholesky_SRCS "*.h")
INSTALL(FILES
${Eigen_Cholesky_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Cholesky
)

35
ground/openpilotgcs/src/libs/eigen/Eigen/src/Cholesky/CholeskyInstantiations.cpp Normal file
View File

@ -0,0 +1,35 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_EXTERN_INSTANTIATIONS
#define EIGEN_EXTERN_INSTANTIATIONS
#endif
#include "../../Core"
#undef EIGEN_EXTERN_INSTANTIATIONS
#include "../../Cholesky"
namespace Eigen {
EIGEN_CHOLESKY_MODULE_INSTANTIATE();
}

192
ground/openpilotgcs/src/libs/eigen/Eigen/src/Cholesky/LDLT.h Normal file
View File

@ -0,0 +1,192 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_LDLT_H
#define EIGEN_LDLT_H
/** \ingroup cholesky_Module
*
* \class LDLT
*
* \brief Robust Cholesky decomposition of a matrix and associated features
*
* \param MatrixType the type of the matrix of which we are computing the LDL^T Cholesky decomposition
*
* This class performs a Cholesky decomposition without square root of a symmetric, positive definite
* matrix A such that A = L D L^* = U^* D U, where L is lower triangular with a unit diagonal
* and D is a diagonal matrix.
*
* Compared to a standard Cholesky decomposition, avoiding the square roots allows for faster and more
* stable computation.
*
* Note that during the decomposition, only the upper triangular part of A is considered. Therefore,
* the strict lower part does not have to store correct values.
*
* \sa MatrixBase::ldlt(), class LLT
*/
template<typename MatrixType> class LDLT
{
public:
typedef typename MatrixType::Scalar Scalar;
typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> VectorType;
LDLT(const MatrixType& matrix)
: m_matrix(matrix.rows(), matrix.cols())
{
compute(matrix);
}
/** \returns the lower triangular matrix L */
inline Part<MatrixType, UnitLowerTriangular> matrixL(void) const { return m_matrix; }
/** \returns the coefficients of the diagonal matrix D */
inline DiagonalCoeffs<MatrixType> vectorD(void) const { return m_matrix.diagonal(); }
/** \returns true if the matrix is positive definite */
inline bool isPositiveDefinite(void) const { return m_isPositiveDefinite; }
template<typename RhsDerived, typename ResultType>
bool solve(const MatrixBase<RhsDerived> &b, ResultType *result) const;
template<typename Derived>
bool solveInPlace(MatrixBase<Derived> &bAndX) const;
void compute(const MatrixType& matrix);
protected:
/** \internal
* Used to compute and store the cholesky decomposition A = L D L^* = U^* D U.
* The strict upper part is used during the decomposition, the strict lower
* part correspond to the coefficients of L (its diagonal is equal to 1 and
* is not stored), and the diagonal entries correspond to D.
*/
MatrixType m_matrix;
bool m_isPositiveDefinite;
};
/** Compute / recompute the LLT decomposition A = L D L^* = U^* D U of \a matrix
*/
template<typename MatrixType>
void LDLT<MatrixType>::compute(const MatrixType& a)
{
assert(a.rows()==a.cols());
const int size = a.rows();
m_matrix.resize(size, size);
m_isPositiveDefinite = true; // always true. This decomposition is not rank-revealing anyway.
if (size<=1)
{
m_matrix = a;
return;
}
// Let's preallocate a temporay vector to evaluate the matrix-vector product into it.
// Unlike the standard LLT decomposition, here we cannot evaluate it to the destination
// matrix because it a sub-row which is not compatible suitable for efficient packet evaluation.
// (at least if we assume the matrix is col-major)
Matrix<Scalar,MatrixType::RowsAtCompileTime,1> _temporary(size);
// Note that, in this algorithm the rows of the strict upper part of m_matrix is used to store
// column vector, thus the strange .conjugate() and .transpose()...
m_matrix.row(0) = a.row(0).conjugate();
m_matrix.col(0).end(size-1) = m_matrix.row(0).end(size-1) / m_matrix.coeff(0,0);
for (int j = 1; j < size; ++j)
{
RealScalar tmp = ei_real(a.coeff(j,j) - (m_matrix.row(j).start(j) * m_matrix.col(j).start(j).conjugate()).coeff(0,0));
m_matrix.coeffRef(j,j) = tmp;
int endSize = size-j-1;
if (endSize>0)
{
_temporary.end(endSize) = ( m_matrix.block(j+1,0, endSize, j)
* m_matrix.col(j).start(j).conjugate() ).lazy();
m_matrix.row(j).end(endSize) = a.row(j).end(endSize).conjugate()
- _temporary.end(endSize).transpose();
if(tmp != RealScalar(0))
m_matrix.col(j).end(endSize) = m_matrix.row(j).end(endSize) / tmp;
}
}
}
/** Computes the solution x of \f$ A x = b \f$ using the current decomposition of A.
* The result is stored in \a result
*
* \returns true in case of success, false otherwise.
*
* In other words, it computes \f$ b = A^{-1} b \f$ with
* \f$ {L^{*}}^{-1} D^{-1} L^{-1} b \f$ from right to left.
*
* \sa LDLT::solveInPlace(), MatrixBase::ldlt()
*/
template<typename MatrixType>
template<typename RhsDerived, typename ResultType>
bool LDLT<MatrixType>
::solve(const MatrixBase<RhsDerived> &b, ResultType *result) const
{
const int size = m_matrix.rows();
ei_assert(size==b.rows() && "LLT::solve(): invalid number of rows of the right hand side matrix b");
*result = b;
return solveInPlace(*result);
}
/** This is the \em in-place version of solve().
*
* \param bAndX represents both the right-hand side matrix b and result x.
*
* This version avoids a copy when the right hand side matrix b is not
* needed anymore.
*
* \sa LDLT::solve(), MatrixBase::ldlt()
*/
template<typename MatrixType>
template<typename Derived>
bool LDLT<MatrixType>::solveInPlace(MatrixBase<Derived> &bAndX) const
{
const int size = m_matrix.rows();
ei_assert(size==bAndX.rows());
if (!m_isPositiveDefinite)
return false;
matrixL().solveTriangularInPlace(bAndX);
bAndX = (m_matrix.cwise().inverse().template part<Diagonal>() * bAndX).lazy();
m_matrix.adjoint().template part<UnitUpperTriangular>().solveTriangularInPlace(bAndX);
return true;
}
/** \cholesky_module
* \returns the Cholesky decomposition without square root of \c *this
*/
template<typename Derived>
inline const LDLT<typename MatrixBase<Derived>::PlainMatrixType>
MatrixBase<Derived>::ldlt() const
{
return LDLT<PlainMatrixType>(derived());
}
#endif // EIGEN_LDLT_H

220
ground/openpilotgcs/src/libs/eigen/Eigen/src/Cholesky/LLT.h Normal file
View File

@ -0,0 +1,220 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_LLT_H
#define EIGEN_LLT_H
/** \ingroup cholesky_Module
*
* \class LLT
*
* \brief Standard Cholesky decomposition (LL^T) of a matrix and associated features
*
* \param MatrixType the type of the matrix of which we are computing the LL^T Cholesky decomposition
*
* This class performs a LL^T Cholesky decomposition of a symmetric, positive definite
* matrix A such that A = LL^* = U^*U, where L is lower triangular.
*
* While the Cholesky decomposition is particularly useful to solve selfadjoint problems like D^*D x = b,
* for that purpose, we recommend the Cholesky decomposition without square root which is more stable
* and even faster. Nevertheless, this standard Cholesky decomposition remains useful in many other
* situations like generalised eigen problems with hermitian matrices.
*
* Remember that Cholesky decompositions are not rank-revealing. This LLT decomposition is only stable on positive definite matrices,
* use LDLT instead for the semidefinite case. Also, do not use a Cholesky decomposition to determine whether a system of equations
* has a solution.
*
* \sa MatrixBase::llt(), class LDLT
*/
/* HEY THIS DOX IS DISABLED BECAUSE THERE's A BUG EITHER HERE OR IN LDLT ABOUT THAT (OR BOTH)
* Note that during the decomposition, only the upper triangular part of A is considered. Therefore,
* the strict lower part does not have to store correct values.
*/
template<typename MatrixType> class LLT
{
private:
typedef typename MatrixType::Scalar Scalar;
typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> VectorType;
enum {
PacketSize = ei_packet_traits<Scalar>::size,
AlignmentMask = int(PacketSize)-1
};
public:
/**
* \brief Default Constructor.
*
* The default constructor is useful in cases in which the user intends to
* perform decompositions via LLT::compute(const MatrixType&).
*/
LLT() : m_matrix(), m_isInitialized(false) {}
LLT(const MatrixType& matrix)
: m_matrix(matrix.rows(), matrix.cols()),
m_isInitialized(false)
{
compute(matrix);
}
/** \returns the lower triangular matrix L */
inline Part<MatrixType, LowerTriangular> matrixL(void) const
{
ei_assert(m_isInitialized && "LLT is not initialized.");
return m_matrix;
}
/** \deprecated */
inline bool isPositiveDefinite(void) const { return m_isInitialized && m_isPositiveDefinite; }
template<typename RhsDerived, typename ResultType>
bool solve(const MatrixBase<RhsDerived> &b, ResultType *result) const;
template<typename Derived>
bool solveInPlace(MatrixBase<Derived> &bAndX) const;
void compute(const MatrixType& matrix);
protected:
/** \internal
* Used to compute and store L
* The strict upper part is not used and even not initialized.
*/
MatrixType m_matrix;
bool m_isInitialized;
bool m_isPositiveDefinite;
};
/** Computes / recomputes the Cholesky decomposition A = LL^* = U^*U of \a matrix
*/
template<typename MatrixType>
void LLT<MatrixType>::compute(const MatrixType& a)
{
assert(a.rows()==a.cols());
m_isPositiveDefinite = true;
const int size = a.rows();
m_matrix.resize(size, size);
// The biggest overall is the point of reference to which further diagonals
// are compared; if any diagonal is negligible compared
// to the largest overall, the algorithm bails. This cutoff is suggested
// in "Analysis of the Cholesky Decomposition of a Semi-definite Matrix" by
// Nicholas J. Higham. Also see "Accuracy and Stability of Numerical
// Algorithms" page 217, also by Higham.
const RealScalar cutoff = machine_epsilon<Scalar>() * size * a.diagonal().cwise().abs().maxCoeff();
RealScalar x;
x = ei_real(a.coeff(0,0));
m_matrix.coeffRef(0,0) = ei_sqrt(x);
if(size==1)
{
m_isInitialized = true;
return;
}
if(ei_real(m_matrix.coeff(0,0))>0)
m_matrix.col(0).end(size-1) = a.row(0).end(size-1).adjoint() / ei_real(m_matrix.coeff(0,0));
for (int j = 1; j < size; ++j)
{
x = ei_real(a.coeff(j,j)) - m_matrix.row(j).start(j).squaredNorm();
if (x <= cutoff)
{
m_isPositiveDefinite = false;
continue;
}
m_matrix.coeffRef(j,j) = x = ei_sqrt(x);
int endSize = size-j-1;
if (endSize>0) {
// Note that when all matrix columns have good alignment, then the following
// product is guaranteed to be optimal with respect to alignment.
m_matrix.col(j).end(endSize) =
(m_matrix.block(j+1, 0, endSize, j) * m_matrix.row(j).start(j).adjoint()).lazy();
// FIXME could use a.col instead of a.row
m_matrix.col(j).end(endSize) = (a.row(j).end(endSize).adjoint()
- m_matrix.col(j).end(endSize) ) / x;
}
}
m_isInitialized = true;
}
/** Computes the solution x of \f$ A x = b \f$ using the current decomposition of A.
* The result is stored in \a result
*
* \returns true always! If you need to check for existence of solutions, use another decomposition like LU, QR, or SVD.
*
* In other words, it computes \f$ b = A^{-1} b \f$ with
* \f$ {L^{*}}^{-1} L^{-1} b \f$ from right to left.
*
* Example: \include LLT_solve.cpp
* Output: \verbinclude LLT_solve.out
*
* \sa LLT::solveInPlace(), MatrixBase::llt()
*/
template<typename MatrixType>
template<typename RhsDerived, typename ResultType>
bool LLT<MatrixType>::solve(const MatrixBase<RhsDerived> &b, ResultType *result) const
{
ei_assert(m_isInitialized && "LLT is not initialized.");
const int size = m_matrix.rows();
ei_assert(size==b.rows() && "LLT::solve(): invalid number of rows of the right hand side matrix b");
return solveInPlace((*result) = b);
}
/** This is the \em in-place version of solve().
*
* \param bAndX represents both the right-hand side matrix b and result x.
*
* \returns true always! If you need to check for existence of solutions, use another decomposition like LU, QR, or SVD.
*
* This version avoids a copy when the right hand side matrix b is not
* needed anymore.
*
* \sa LLT::solve(), MatrixBase::llt()
*/
template<typename MatrixType>
template<typename Derived>
bool LLT<MatrixType>::solveInPlace(MatrixBase<Derived> &bAndX) const
{
ei_assert(m_isInitialized && "LLT is not initialized.");
const int size = m_matrix.rows();
ei_assert(size==bAndX.rows());
matrixL().solveTriangularInPlace(bAndX);
m_matrix.adjoint().template part<UpperTriangular>().solveTriangularInPlace(bAndX);
return true;
}
/** \cholesky_module
* \returns the LLT decomposition of \c *this
*/
template<typename Derived>
inline const LLT<typename MatrixBase<Derived>::PlainMatrixType>
MatrixBase<Derived>::llt() const
{
return LLT<PlainMatrixType>(derived());
}
#endif // EIGEN_LLT_H

470
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Assign.h Normal file
View File

@ -0,0 +1,470 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2007 Michael Olbrich <michael.olbrich@gmx.net>
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_ASSIGN_H
#define EIGEN_ASSIGN_H
/***************************************************************************
* Part 1 : the logic deciding a strategy for vectorization and unrolling
***************************************************************************/
template <typename Derived, typename OtherDerived>
struct ei_assign_traits
{
public:
enum {
DstIsAligned = Derived::Flags & AlignedBit,
SrcIsAligned = OtherDerived::Flags & AlignedBit,
SrcAlignment = DstIsAligned && SrcIsAligned ? Aligned : Unaligned
};
private:
enum {
InnerSize = int(Derived::Flags)&RowMajorBit
? Derived::ColsAtCompileTime
: Derived::RowsAtCompileTime,
InnerMaxSize = int(Derived::Flags)&RowMajorBit
? Derived::MaxColsAtCompileTime
: Derived::MaxRowsAtCompileTime,
PacketSize = ei_packet_traits<typename Derived::Scalar>::size
};
enum {
MightVectorize = (int(Derived::Flags) & int(OtherDerived::Flags) & ActualPacketAccessBit)
&& ((int(Derived::Flags)&RowMajorBit)==(int(OtherDerived::Flags)&RowMajorBit)),
MayInnerVectorize = MightVectorize && int(InnerSize)!=Dynamic && int(InnerSize)%int(PacketSize)==0
&& int(DstIsAligned) && int(SrcIsAligned),
MayLinearVectorize = MightVectorize && (int(Derived::Flags) & int(OtherDerived::Flags) & LinearAccessBit),
MaySliceVectorize = MightVectorize && int(InnerMaxSize)>=3*PacketSize /* slice vectorization can be slow, so we only
want it if the slices are big, which is indicated by InnerMaxSize rather than InnerSize, think of the case
of a dynamic block in a fixed-size matrix */
};
public:
enum {
Vectorization = int(MayInnerVectorize) ? int(InnerVectorization)
: int(MayLinearVectorize) ? int(LinearVectorization)
: int(MaySliceVectorize) ? int(SliceVectorization)
: int(NoVectorization)
};
private:
enum {
UnrollingLimit = EIGEN_UNROLLING_LIMIT * (int(Vectorization) == int(NoVectorization) ? 1 : int(PacketSize)),
MayUnrollCompletely = int(Derived::SizeAtCompileTime) * int(OtherDerived::CoeffReadCost) <= int(UnrollingLimit),
MayUnrollInner = int(InnerSize * OtherDerived::CoeffReadCost) <= int(UnrollingLimit)
};
public:
enum {
Unrolling = (int(Vectorization) == int(InnerVectorization) || int(Vectorization) == int(NoVectorization))
? (
int(MayUnrollCompletely) ? int(CompleteUnrolling)
: int(MayUnrollInner) ? int(InnerUnrolling)
: int(NoUnrolling)
)
: int(Vectorization) == int(LinearVectorization)
? ( int(MayUnrollCompletely) && int(DstIsAligned) ? int(CompleteUnrolling) : int(NoUnrolling) )
: int(NoUnrolling)
};
#ifdef EIGEN_DEBUG_ASSIGN
#define EIGEN_DEBUG_VAR(x) std::cerr << #x << " = " << x << std::endl;
static void debug()
{
EIGEN_DEBUG_VAR(DstIsAligned)
EIGEN_DEBUG_VAR(SrcIsAligned)
EIGEN_DEBUG_VAR(SrcAlignment)
EIGEN_DEBUG_VAR(InnerSize)
EIGEN_DEBUG_VAR(InnerMaxSize)
EIGEN_DEBUG_VAR(PacketSize)
EIGEN_DEBUG_VAR(MightVectorize)
EIGEN_DEBUG_VAR(MayInnerVectorize)
EIGEN_DEBUG_VAR(MayLinearVectorize)
EIGEN_DEBUG_VAR(MaySliceVectorize)
EIGEN_DEBUG_VAR(Vectorization)
EIGEN_DEBUG_VAR(UnrollingLimit)
EIGEN_DEBUG_VAR(MayUnrollCompletely)
EIGEN_DEBUG_VAR(MayUnrollInner)
EIGEN_DEBUG_VAR(Unrolling)
}
#endif
};
/***************************************************************************
* Part 2 : meta-unrollers
***************************************************************************/
/***********************
*** No vectorization ***
***********************/
template<typename Derived1, typename Derived2, int Index, int Stop>
struct ei_assign_novec_CompleteUnrolling
{
enum {
row = int(Derived1::Flags)&RowMajorBit
? Index / int(Derived1::ColsAtCompileTime)
: Index % Derived1::RowsAtCompileTime,
col = int(Derived1::Flags)&RowMajorBit
? Index % int(Derived1::ColsAtCompileTime)
: Index / Derived1::RowsAtCompileTime
};
EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src)
{
dst.copyCoeff(row, col, src);
ei_assign_novec_CompleteUnrolling<Derived1, Derived2, Index+1, Stop>::run(dst, src);
}
};
template<typename Derived1, typename Derived2, int Stop>
struct ei_assign_novec_CompleteUnrolling<Derived1, Derived2, Stop, Stop>
{
EIGEN_STRONG_INLINE static void run(Derived1 &, const Derived2 &) {}
};
template<typename Derived1, typename Derived2, int Index, int Stop>
struct ei_assign_novec_InnerUnrolling
{
EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src, int row_or_col)
{
const bool rowMajor = int(Derived1::Flags)&RowMajorBit;
const int row = rowMajor ? row_or_col : Index;
const int col = rowMajor ? Index : row_or_col;
dst.copyCoeff(row, col, src);
ei_assign_novec_InnerUnrolling<Derived1, Derived2, Index+1, Stop>::run(dst, src, row_or_col);
}
};
template<typename Derived1, typename Derived2, int Stop>
struct ei_assign_novec_InnerUnrolling<Derived1, Derived2, Stop, Stop>
{
EIGEN_STRONG_INLINE static void run(Derived1 &, const Derived2 &, int) {}
};
/**************************
*** Inner vectorization ***
**************************/
template<typename Derived1, typename Derived2, int Index, int Stop>
struct ei_assign_innervec_CompleteUnrolling
{
enum {
row = int(Derived1::Flags)&RowMajorBit
? Index / int(Derived1::ColsAtCompileTime)
: Index % Derived1::RowsAtCompileTime,
col = int(Derived1::Flags)&RowMajorBit
? Index % int(Derived1::ColsAtCompileTime)
: Index / Derived1::RowsAtCompileTime,
SrcAlignment = ei_assign_traits<Derived1,Derived2>::SrcAlignment
};
EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src)
{
dst.template copyPacket<Derived2, Aligned, SrcAlignment>(row, col, src);
ei_assign_innervec_CompleteUnrolling<Derived1, Derived2,
Index+ei_packet_traits<typename Derived1::Scalar>::size, Stop>::run(dst, src);
}
};
template<typename Derived1, typename Derived2, int Stop>
struct ei_assign_innervec_CompleteUnrolling<Derived1, Derived2, Stop, Stop>
{
EIGEN_STRONG_INLINE static void run(Derived1 &, const Derived2 &) {}
};
template<typename Derived1, typename Derived2, int Index, int Stop>
struct ei_assign_innervec_InnerUnrolling
{
EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src, int row_or_col)
{
const int row = int(Derived1::Flags)&RowMajorBit ? row_or_col : Index;
const int col = int(Derived1::Flags)&RowMajorBit ? Index : row_or_col;
dst.template copyPacket<Derived2, Aligned, Aligned>(row, col, src);
ei_assign_innervec_InnerUnrolling<Derived1, Derived2,
Index+ei_packet_traits<typename Derived1::Scalar>::size, Stop>::run(dst, src, row_or_col);
}
};
template<typename Derived1, typename Derived2, int Stop>
struct ei_assign_innervec_InnerUnrolling<Derived1, Derived2, Stop, Stop>
{
EIGEN_STRONG_INLINE static void run(Derived1 &, const Derived2 &, int) {}
};
/***************************************************************************
* Part 3 : implementation of all cases
***************************************************************************/
template<typename Derived1, typename Derived2,
int Vectorization = ei_assign_traits<Derived1, Derived2>::Vectorization,
int Unrolling = ei_assign_traits<Derived1, Derived2>::Unrolling>
struct ei_assign_impl;
/***********************
*** No vectorization ***
***********************/
template<typename Derived1, typename Derived2>
struct ei_assign_impl<Derived1, Derived2, NoVectorization, NoUnrolling>
{
inline static void run(Derived1 &dst, const Derived2 &src)
{
const int innerSize = dst.innerSize();
const int outerSize = dst.outerSize();
for(int j = 0; j < outerSize; ++j)
for(int i = 0; i < innerSize; ++i)
{
if(int(Derived1::Flags)&RowMajorBit)
dst.copyCoeff(j, i, src);
else
dst.copyCoeff(i, j, src);
}
}
};
template<typename Derived1, typename Derived2>
struct ei_assign_impl<Derived1, Derived2, NoVectorization, CompleteUnrolling>
{
EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src)
{
ei_assign_novec_CompleteUnrolling<Derived1, Derived2, 0, Derived1::SizeAtCompileTime>
::run(dst, src);
}
};
template<typename Derived1, typename Derived2>
struct ei_assign_impl<Derived1, Derived2, NoVectorization, InnerUnrolling>
{
EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src)
{
const bool rowMajor = int(Derived1::Flags)&RowMajorBit;
const int innerSize = rowMajor ? Derived1::ColsAtCompileTime : Derived1::RowsAtCompileTime;
const int outerSize = dst.outerSize();
for(int j = 0; j < outerSize; ++j)
ei_assign_novec_InnerUnrolling<Derived1, Derived2, 0, innerSize>
::run(dst, src, j);
}
};
/**************************
*** Inner vectorization ***
**************************/
template<typename Derived1, typename Derived2>
struct ei_assign_impl<Derived1, Derived2, InnerVectorization, NoUnrolling>
{
inline static void run(Derived1 &dst, const Derived2 &src)
{
const int innerSize = dst.innerSize();
const int outerSize = dst.outerSize();
const int packetSize = ei_packet_traits<typename Derived1::Scalar>::size;
for(int j = 0; j < outerSize; ++j)
for(int i = 0; i < innerSize; i+=packetSize)
{
if(int(Derived1::Flags)&RowMajorBit)
dst.template copyPacket<Derived2, Aligned, Aligned>(j, i, src);
else
dst.template copyPacket<Derived2, Aligned, Aligned>(i, j, src);
}
}
};
template<typename Derived1, typename Derived2>
struct ei_assign_impl<Derived1, Derived2, InnerVectorization, CompleteUnrolling>
{
EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src)
{
ei_assign_innervec_CompleteUnrolling<Derived1, Derived2, 0, Derived1::SizeAtCompileTime>
::run(dst, src);
}
};
template<typename Derived1, typename Derived2>
struct ei_assign_impl<Derived1, Derived2, InnerVectorization, InnerUnrolling>
{
EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src)
{
const bool rowMajor = int(Derived1::Flags)&RowMajorBit;
const int innerSize = rowMajor ? Derived1::ColsAtCompileTime : Derived1::RowsAtCompileTime;
const int outerSize = dst.outerSize();
for(int j = 0; j < outerSize; ++j)
ei_assign_innervec_InnerUnrolling<Derived1, Derived2, 0, innerSize>
::run(dst, src, j);
}
};
/***************************
*** Linear vectorization ***
***************************/
template<typename Derived1, typename Derived2>
struct ei_assign_impl<Derived1, Derived2, LinearVectorization, NoUnrolling>
{
inline static void run(Derived1 &dst, const Derived2 &src)
{
const int size = dst.size();
const int packetSize = ei_packet_traits<typename Derived1::Scalar>::size;
const int alignedStart = ei_assign_traits<Derived1,Derived2>::DstIsAligned ? 0
: ei_alignmentOffset(&dst.coeffRef(0), size);
const int alignedEnd = alignedStart + ((size-alignedStart)/packetSize)*packetSize;
for(int index = 0; index < alignedStart; ++index)
dst.copyCoeff(index, src);
for(int index = alignedStart; index < alignedEnd; index += packetSize)
{
dst.template copyPacket<Derived2, Aligned, ei_assign_traits<Derived1,Derived2>::SrcAlignment>(index, src);
}
for(int index = alignedEnd; index < size; ++index)
dst.copyCoeff(index, src);
}
};
template<typename Derived1, typename Derived2>
struct ei_assign_impl<Derived1, Derived2, LinearVectorization, CompleteUnrolling>
{
EIGEN_STRONG_INLINE static void run(Derived1 &dst, const Derived2 &src)
{
const int size = Derived1::SizeAtCompileTime;
const int packetSize = ei_packet_traits<typename Derived1::Scalar>::size;
const int alignedSize = (size/packetSize)*packetSize;
ei_assign_innervec_CompleteUnrolling<Derived1, Derived2, 0, alignedSize>::run(dst, src);
ei_assign_novec_CompleteUnrolling<Derived1, Derived2, alignedSize, size>::run(dst, src);
}
};
/**************************
*** Slice vectorization ***
***************************/
template<typename Derived1, typename Derived2>
struct ei_assign_impl<Derived1, Derived2, SliceVectorization, NoUnrolling>
{
inline static void run(Derived1 &dst, const Derived2 &src)
{
const int packetSize = ei_packet_traits<typename Derived1::Scalar>::size;
const int packetAlignedMask = packetSize - 1;
const int innerSize = dst.innerSize();
const int outerSize = dst.outerSize();
const int alignedStep = (packetSize - dst.stride() % packetSize) & packetAlignedMask;
int alignedStart = ei_assign_traits<Derived1,Derived2>::DstIsAligned ? 0
: ei_alignmentOffset(&dst.coeffRef(0,0), innerSize);
for(int i = 0; i < outerSize; ++i)
{
const int alignedEnd = alignedStart + ((innerSize-alignedStart) & ~packetAlignedMask);
// do the non-vectorizable part of the assignment
for (int index = 0; index<alignedStart ; ++index)
{
if(Derived1::Flags&RowMajorBit)
dst.copyCoeff(i, index, src);
else
dst.copyCoeff(index, i, src);
}
// do the vectorizable part of the assignment
for (int index = alignedStart; index<alignedEnd; index+=packetSize)
{
if(Derived1::Flags&RowMajorBit)
dst.template copyPacket<Derived2, Aligned, Unaligned>(i, index, src);
else
dst.template copyPacket<Derived2, Aligned, Unaligned>(index, i, src);
}
// do the non-vectorizable part of the assignment
for (int index = alignedEnd; index<innerSize ; ++index)
{
if(Derived1::Flags&RowMajorBit)
dst.copyCoeff(i, index, src);
else
dst.copyCoeff(index, i, src);
}
alignedStart = std::min<int>((alignedStart+alignedStep)%packetSize, innerSize);
}
}
};
/***************************************************************************
* Part 4 : implementation of MatrixBase methods
***************************************************************************/
template<typename Derived>
template<typename OtherDerived>
EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>
::lazyAssign(const MatrixBase<OtherDerived>& other)
{
#ifdef EIGEN_DEBUG_ASSIGN
ei_assign_traits<Derived, OtherDerived>::debug();
#endif
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived,OtherDerived)
EIGEN_STATIC_ASSERT((ei_is_same_type<typename Derived::Scalar, typename OtherDerived::Scalar>::ret),
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
ei_assert(rows() == other.rows() && cols() == other.cols());
ei_assign_impl<Derived, OtherDerived>::run(derived(),other.derived());
return derived();
}
template<typename Derived, typename OtherDerived,
bool EvalBeforeAssigning = (int(OtherDerived::Flags) & EvalBeforeAssigningBit) != 0,
bool NeedToTranspose = Derived::IsVectorAtCompileTime
&& OtherDerived::IsVectorAtCompileTime
&& int(Derived::RowsAtCompileTime) == int(OtherDerived::ColsAtCompileTime)
&& int(Derived::ColsAtCompileTime) == int(OtherDerived::RowsAtCompileTime)
&& int(Derived::SizeAtCompileTime) != 1>
struct ei_assign_selector;
template<typename Derived, typename OtherDerived>
struct ei_assign_selector<Derived,OtherDerived,false,false> {
EIGEN_STRONG_INLINE static Derived& run(Derived& dst, const OtherDerived& other) { return dst.lazyAssign(other.derived()); }
};
template<typename Derived, typename OtherDerived>
struct ei_assign_selector<Derived,OtherDerived,true,false> {
EIGEN_STRONG_INLINE static Derived& run(Derived& dst, const OtherDerived& other) { return dst.lazyAssign(other.eval()); }
};
template<typename Derived, typename OtherDerived>
struct ei_assign_selector<Derived,OtherDerived,false,true> {
EIGEN_STRONG_INLINE static Derived& run(Derived& dst, const OtherDerived& other) { return dst.lazyAssign(other.transpose()); }
};
template<typename Derived, typename OtherDerived>
struct ei_assign_selector<Derived,OtherDerived,true,true> {
EIGEN_STRONG_INLINE static Derived& run(Derived& dst, const OtherDerived& other) { return dst.lazyAssign(other.transpose().eval()); }
};
template<typename Derived>
template<typename OtherDerived>
EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>
::operator=(const MatrixBase<OtherDerived>& other)
{
return ei_assign_selector<Derived,OtherDerived>::run(derived(), other.derived());
}
#endif // EIGEN_ASSIGN_H

752
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Block.h Normal file
View File

@ -0,0 +1,752 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_BLOCK_H
#define EIGEN_BLOCK_H
/** \class Block
*
* \brief Expression of a fixed-size or dynamic-size block
*
* \param MatrixType the type of the object in which we are taking a block
* \param BlockRows the number of rows of the block we are taking at compile time (optional)
* \param BlockCols the number of columns of the block we are taking at compile time (optional)
* \param _PacketAccess allows to enforce aligned loads and stores if set to ForceAligned.
* The default is AsRequested. This parameter is internaly used by Eigen
* in expressions such as \code mat.block() += other; \endcode and most of
* the time this is the only way it is used.
* \param _DirectAccessStatus \internal used for partial specialization
*
* This class represents an expression of either a fixed-size or dynamic-size block. It is the return
* type of MatrixBase::block(int,int,int,int) and MatrixBase::block<int,int>(int,int) and
* most of the time this is the only way it is used.
*
* However, if you want to directly maniputate block expressions,
* for instance if you want to write a function returning such an expression, you
* will need to use this class.
*
* Here is an example illustrating the dynamic case:
* \include class_Block.cpp
* Output: \verbinclude class_Block.out
*
* \note Even though this expression has dynamic size, in the case where \a MatrixType
* has fixed size, this expression inherits a fixed maximal size which means that evaluating
* it does not cause a dynamic memory allocation.
*
* Here is an example illustrating the fixed-size case:
* \include class_FixedBlock.cpp
* Output: \verbinclude class_FixedBlock.out
*
* \sa MatrixBase::block(int,int,int,int), MatrixBase::block(int,int), class VectorBlock
*/
template<typename MatrixType, int BlockRows, int BlockCols, int _PacketAccess, int _DirectAccessStatus>
struct ei_traits<Block<MatrixType, BlockRows, BlockCols, _PacketAccess, _DirectAccessStatus> >
{
typedef typename ei_traits<MatrixType>::Scalar Scalar;
typedef typename ei_nested<MatrixType>::type MatrixTypeNested;
typedef typename ei_unref<MatrixTypeNested>::type _MatrixTypeNested;
enum{
RowsAtCompileTime = ei_traits<MatrixType>::RowsAtCompileTime == 1 ? 1 : BlockRows,
ColsAtCompileTime = ei_traits<MatrixType>::ColsAtCompileTime == 1 ? 1 : BlockCols,
MaxRowsAtCompileTime = RowsAtCompileTime == 1 ? 1
: (BlockRows==Dynamic ? int(ei_traits<MatrixType>::MaxRowsAtCompileTime) : BlockRows),
MaxColsAtCompileTime = ColsAtCompileTime == 1 ? 1
: (BlockCols==Dynamic ? int(ei_traits<MatrixType>::MaxColsAtCompileTime) : BlockCols),
RowMajor = int(ei_traits<MatrixType>::Flags)&RowMajorBit,
InnerSize = RowMajor ? int(ColsAtCompileTime) : int(RowsAtCompileTime),
InnerMaxSize = RowMajor ? int(MaxColsAtCompileTime) : int(MaxRowsAtCompileTime),
MaskPacketAccessBit = (InnerMaxSize == Dynamic || (InnerSize >= ei_packet_traits<Scalar>::size))
? PacketAccessBit : 0,
FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1) ? LinearAccessBit : 0,
Flags = (ei_traits<MatrixType>::Flags & (HereditaryBits | MaskPacketAccessBit | DirectAccessBit)) | FlagsLinearAccessBit,
CoeffReadCost = ei_traits<MatrixType>::CoeffReadCost,
PacketAccess = _PacketAccess
};
typedef typename ei_meta_if<int(PacketAccess)==ForceAligned,
Block<MatrixType, BlockRows, BlockCols, _PacketAccess, _DirectAccessStatus>&,
Block<MatrixType, BlockRows, BlockCols, ForceAligned, _DirectAccessStatus> >::ret AlignedDerivedType;
};
template<typename MatrixType, int BlockRows, int BlockCols, int PacketAccess, int _DirectAccessStatus> class Block
: public MatrixBase<Block<MatrixType, BlockRows, BlockCols, PacketAccess, _DirectAccessStatus> >
{
public:
EIGEN_GENERIC_PUBLIC_INTERFACE(Block)
class InnerIterator;
/** Column or Row constructor
*/
inline Block(const MatrixType& matrix, int i)
: m_matrix(matrix),
// It is a row if and only if BlockRows==1 and BlockCols==MatrixType::ColsAtCompileTime,
// and it is a column if and only if BlockRows==MatrixType::RowsAtCompileTime and BlockCols==1,
// all other cases are invalid.
// The case a 1x1 matrix seems ambiguous, but the result is the same anyway.
m_startRow( (BlockRows==1) && (BlockCols==MatrixType::ColsAtCompileTime) ? i : 0),
m_startCol( (BlockRows==MatrixType::RowsAtCompileTime) && (BlockCols==1) ? i : 0),
m_blockRows(matrix.rows()), // if it is a row, then m_blockRows has a fixed-size of 1, so no pb to try to overwrite it
m_blockCols(matrix.cols()) // same for m_blockCols
{
ei_assert( (i>=0) && (
((BlockRows==1) && (BlockCols==MatrixType::ColsAtCompileTime) && i<matrix.rows())
||((BlockRows==MatrixType::RowsAtCompileTime) && (BlockCols==1) && i<matrix.cols())));
}
/** Fixed-size constructor
*/
inline Block(const MatrixType& matrix, int startRow, int startCol)
: m_matrix(matrix), m_startRow(startRow), m_startCol(startCol),
m_blockRows(matrix.rows()), m_blockCols(matrix.cols())
{
EIGEN_STATIC_ASSERT(RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic,THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE)
ei_assert(startRow >= 0 && BlockRows >= 1 && startRow + BlockRows <= matrix.rows()
&& startCol >= 0 && BlockCols >= 1 && startCol + BlockCols <= matrix.cols());
}
/** Dynamic-size constructor
*/
inline Block(const MatrixType& matrix,
int startRow, int startCol,
int blockRows, int blockCols)
: m_matrix(matrix), m_startRow(startRow), m_startCol(startCol),
m_blockRows(blockRows), m_blockCols(blockCols)
{
ei_assert((RowsAtCompileTime==Dynamic || RowsAtCompileTime==blockRows)
&& (ColsAtCompileTime==Dynamic || ColsAtCompileTime==blockCols));
ei_assert(startRow >= 0 && blockRows >= 1 && startRow + blockRows <= matrix.rows()
&& startCol >= 0 && blockCols >= 1 && startCol + blockCols <= matrix.cols());
}
EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Block)
inline int rows() const { return m_blockRows.value(); }
inline int cols() const { return m_blockCols.value(); }
inline Scalar& coeffRef(int row, int col)
{
return m_matrix.const_cast_derived()
.coeffRef(row + m_startRow.value(), col + m_startCol.value());
}
inline const Scalar coeff(int row, int col) const
{
return m_matrix.coeff(row + m_startRow.value(), col + m_startCol.value());
}
inline Scalar& coeffRef(int index)
{
return m_matrix.const_cast_derived()
.coeffRef(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
}
inline const Scalar coeff(int index) const
{
return m_matrix
.coeff(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
}
template<int LoadMode>
inline PacketScalar packet(int row, int col) const
{
return m_matrix.template packet<Unaligned>
(row + m_startRow.value(), col + m_startCol.value());
}
template<int LoadMode>
inline void writePacket(int row, int col, const PacketScalar& x)
{
m_matrix.const_cast_derived().template writePacket<Unaligned>
(row + m_startRow.value(), col + m_startCol.value(), x);
}
template<int LoadMode>
inline PacketScalar packet(int index) const
{
return m_matrix.template packet<Unaligned>
(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0));
}
template<int LoadMode>
inline void writePacket(int index, const PacketScalar& x)
{
m_matrix.const_cast_derived().template writePacket<Unaligned>
(m_startRow.value() + (RowsAtCompileTime == 1 ? 0 : index),
m_startCol.value() + (RowsAtCompileTime == 1 ? index : 0), x);
}
protected:
const typename MatrixType::Nested m_matrix;
const ei_int_if_dynamic<MatrixType::RowsAtCompileTime == 1 ? 0 : Dynamic> m_startRow;
const ei_int_if_dynamic<MatrixType::ColsAtCompileTime == 1 ? 0 : Dynamic> m_startCol;
const ei_int_if_dynamic<RowsAtCompileTime> m_blockRows;
const ei_int_if_dynamic<ColsAtCompileTime> m_blockCols;
};
/** \internal */
template<typename MatrixType, int BlockRows, int BlockCols, int PacketAccess>
class Block<MatrixType,BlockRows,BlockCols,PacketAccess,HasDirectAccess>
: public MapBase<Block<MatrixType, BlockRows, BlockCols,PacketAccess,HasDirectAccess> >
{
public:
_EIGEN_GENERIC_PUBLIC_INTERFACE(Block, MapBase<Block>)
class InnerIterator;
typedef typename ei_traits<Block>::AlignedDerivedType AlignedDerivedType;
friend class Block<MatrixType,BlockRows,BlockCols,PacketAccess==int(AsRequested)?ForceAligned:AsRequested,HasDirectAccess>;
EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Block)
AlignedDerivedType _convertToForceAligned()
{
return Block<MatrixType,BlockRows,BlockCols,ForceAligned,HasDirectAccess>
(m_matrix, Base::m_data, Base::m_rows.value(), Base::m_cols.value());
}
/** Column or Row constructor
*/
inline Block(const MatrixType& matrix, int i)
: Base(&matrix.const_cast_derived().coeffRef(
(BlockRows==1) && (BlockCols==MatrixType::ColsAtCompileTime) ? i : 0,
(BlockRows==MatrixType::RowsAtCompileTime) && (BlockCols==1) ? i : 0),
BlockRows==1 ? 1 : matrix.rows(),
BlockCols==1 ? 1 : matrix.cols()),
m_matrix(matrix)
{
ei_assert( (i>=0) && (
((BlockRows==1) && (BlockCols==MatrixType::ColsAtCompileTime) && i<matrix.rows())
||((BlockRows==MatrixType::RowsAtCompileTime) && (BlockCols==1) && i<matrix.cols())));
}
/** Fixed-size constructor
*/
inline Block(const MatrixType& matrix, int startRow, int startCol)
: Base(&matrix.const_cast_derived().coeffRef(startRow,startCol)), m_matrix(matrix)
{
ei_assert(startRow >= 0 && BlockRows >= 1 && startRow + BlockRows <= matrix.rows()
&& startCol >= 0 && BlockCols >= 1 && startCol + BlockCols <= matrix.cols());
}
/** Dynamic-size constructor
*/
inline Block(const MatrixType& matrix,
int startRow, int startCol,
int blockRows, int blockCols)
: Base(&matrix.const_cast_derived().coeffRef(startRow,startCol), blockRows, blockCols),
m_matrix(matrix)
{
ei_assert((RowsAtCompileTime==Dynamic || RowsAtCompileTime==blockRows)
&& (ColsAtCompileTime==Dynamic || ColsAtCompileTime==blockCols));
ei_assert(startRow >= 0 && blockRows >= 1 && startRow + blockRows <= matrix.rows()
&& startCol >= 0 && blockCols >= 1 && startCol + blockCols <= matrix.cols());
}
inline int stride(void) const { return m_matrix.stride(); }
protected:
/** \internal used by allowAligned() */
inline Block(const MatrixType& matrix, const Scalar* data, int blockRows, int blockCols)
: Base(data, blockRows, blockCols), m_matrix(matrix)
{}
const typename MatrixType::Nested m_matrix;
};
/** \returns a dynamic-size expression of a block in *this.
*
* \param startRow the first row in the block
* \param startCol the first column in the block
* \param blockRows the number of rows in the block
* \param blockCols the number of columns in the block
*
* \addexample BlockIntIntIntInt \label How to reference a sub-matrix (dynamic-size)
*
* Example: \include MatrixBase_block_int_int_int_int.cpp
* Output: \verbinclude MatrixBase_block_int_int_int_int.out
*
* \note Even though the returned expression has dynamic size, in the case
* when it is applied to a fixed-size matrix, it inherits a fixed maximal size,
* which means that evaluating it does not cause a dynamic memory allocation.
*
* \sa class Block, block(int,int)
*/
template<typename Derived>
inline typename BlockReturnType<Derived>::Type MatrixBase<Derived>
::block(int startRow, int startCol, int blockRows, int blockCols)
{
return typename BlockReturnType<Derived>::Type(derived(), startRow, startCol, blockRows, blockCols);
}
/** This is the const version of block(int,int,int,int). */
template<typename Derived>
inline const typename BlockReturnType<Derived>::Type MatrixBase<Derived>
::block(int startRow, int startCol, int blockRows, int blockCols) const
{
return typename BlockReturnType<Derived>::Type(derived(), startRow, startCol, blockRows, blockCols);
}
/** \returns a dynamic-size expression of a segment (i.e. a vector block) in *this.
*
* \only_for_vectors
*
* \addexample SegmentIntInt \label How to reference a sub-vector (dynamic size)
*
* \param start the first coefficient in the segment
* \param size the number of coefficients in the segment
*
* Example: \include MatrixBase_segment_int_int.cpp
* Output: \verbinclude MatrixBase_segment_int_int.out
*
* \note Even though the returned expression has dynamic size, in the case
* when it is applied to a fixed-size vector, it inherits a fixed maximal size,
* which means that evaluating it does not cause a dynamic memory allocation.
*
* \sa class Block, segment(int)
*/
template<typename Derived>
inline typename BlockReturnType<Derived>::SubVectorType MatrixBase<Derived>
::segment(int start, int size)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return typename BlockReturnType<Derived>::SubVectorType(derived(), RowsAtCompileTime == 1 ? 0 : start,
ColsAtCompileTime == 1 ? 0 : start,
RowsAtCompileTime == 1 ? 1 : size,
ColsAtCompileTime == 1 ? 1 : size);
}
/** This is the const version of segment(int,int).*/
template<typename Derived>
inline const typename BlockReturnType<Derived>::SubVectorType
MatrixBase<Derived>::segment(int start, int size) const
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return typename BlockReturnType<Derived>::SubVectorType(derived(), RowsAtCompileTime == 1 ? 0 : start,
ColsAtCompileTime == 1 ? 0 : start,
RowsAtCompileTime == 1 ? 1 : size,
ColsAtCompileTime == 1 ? 1 : size);
}
/** \returns a dynamic-size expression of the first coefficients of *this.
*
* \only_for_vectors
*
* \param size the number of coefficients in the block
*
* \addexample BlockInt \label How to reference a sub-vector (fixed-size)
*
* Example: \include MatrixBase_start_int.cpp
* Output: \verbinclude MatrixBase_start_int.out
*
* \note Even though the returned expression has dynamic size, in the case
* when it is applied to a fixed-size vector, it inherits a fixed maximal size,
* which means that evaluating it does not cause a dynamic memory allocation.
*
* \sa class Block, block(int,int)
*/
template<typename Derived>
inline typename BlockReturnType<Derived,Dynamic>::SubVectorType
MatrixBase<Derived>::start(int size)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return Block<Derived,
RowsAtCompileTime == 1 ? 1 : Dynamic,
ColsAtCompileTime == 1 ? 1 : Dynamic>
(derived(), 0, 0,
RowsAtCompileTime == 1 ? 1 : size,
ColsAtCompileTime == 1 ? 1 : size);
}
/** This is the const version of start(int).*/
template<typename Derived>
inline const typename BlockReturnType<Derived,Dynamic>::SubVectorType
MatrixBase<Derived>::start(int size) const
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return Block<Derived,
RowsAtCompileTime == 1 ? 1 : Dynamic,
ColsAtCompileTime == 1 ? 1 : Dynamic>
(derived(), 0, 0,
RowsAtCompileTime == 1 ? 1 : size,
ColsAtCompileTime == 1 ? 1 : size);
}
/** \returns a dynamic-size expression of the last coefficients of *this.
*
* \only_for_vectors
*
* \param size the number of coefficients in the block
*
* \addexample BlockEnd \label How to reference the end of a vector (fixed-size)
*
* Example: \include MatrixBase_end_int.cpp
* Output: \verbinclude MatrixBase_end_int.out
*
* \note Even though the returned expression has dynamic size, in the case
* when it is applied to a fixed-size vector, it inherits a fixed maximal size,
* which means that evaluating it does not cause a dynamic memory allocation.
*
* \sa class Block, block(int,int)
*/
template<typename Derived>
inline typename BlockReturnType<Derived,Dynamic>::SubVectorType
MatrixBase<Derived>::end(int size)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return Block<Derived,
RowsAtCompileTime == 1 ? 1 : Dynamic,
ColsAtCompileTime == 1 ? 1 : Dynamic>
(derived(),
RowsAtCompileTime == 1 ? 0 : rows() - size,
ColsAtCompileTime == 1 ? 0 : cols() - size,
RowsAtCompileTime == 1 ? 1 : size,
ColsAtCompileTime == 1 ? 1 : size);
}
/** This is the const version of end(int).*/
template<typename Derived>
inline const typename BlockReturnType<Derived,Dynamic>::SubVectorType
MatrixBase<Derived>::end(int size) const
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return Block<Derived,
RowsAtCompileTime == 1 ? 1 : Dynamic,
ColsAtCompileTime == 1 ? 1 : Dynamic>
(derived(),
RowsAtCompileTime == 1 ? 0 : rows() - size,
ColsAtCompileTime == 1 ? 0 : cols() - size,
RowsAtCompileTime == 1 ? 1 : size,
ColsAtCompileTime == 1 ? 1 : size);
}
/** \returns a fixed-size expression of a segment (i.e. a vector block) in \c *this
*
* \only_for_vectors
*
* The template parameter \a Size is the number of coefficients in the block
*
* \param start the index of the first element of the sub-vector
*
* Example: \include MatrixBase_template_int_segment.cpp
* Output: \verbinclude MatrixBase_template_int_segment.out
*
* \sa class Block
*/
template<typename Derived>
template<int Size>
inline typename BlockReturnType<Derived,Size>::SubVectorType
MatrixBase<Derived>::segment(int start)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return Block<Derived, (RowsAtCompileTime == 1 ? 1 : Size),
(ColsAtCompileTime == 1 ? 1 : Size)>
(derived(), RowsAtCompileTime == 1 ? 0 : start,
ColsAtCompileTime == 1 ? 0 : start);
}
/** This is the const version of segment<int>(int).*/
template<typename Derived>
template<int Size>
inline const typename BlockReturnType<Derived,Size>::SubVectorType
MatrixBase<Derived>::segment(int start) const
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return Block<Derived, (RowsAtCompileTime == 1 ? 1 : Size),
(ColsAtCompileTime == 1 ? 1 : Size)>
(derived(), RowsAtCompileTime == 1 ? 0 : start,
ColsAtCompileTime == 1 ? 0 : start);
}
/** \returns a fixed-size expression of the first coefficients of *this.
*
* \only_for_vectors
*
* The template parameter \a Size is the number of coefficients in the block
*
* \addexample BlockStart \label How to reference the start of a vector (fixed-size)
*
* Example: \include MatrixBase_template_int_start.cpp
* Output: \verbinclude MatrixBase_template_int_start.out
*
* \sa class Block
*/
template<typename Derived>
template<int Size>
inline typename BlockReturnType<Derived,Size>::SubVectorType
MatrixBase<Derived>::start()
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return Block<Derived, (RowsAtCompileTime == 1 ? 1 : Size),
(ColsAtCompileTime == 1 ? 1 : Size)>(derived(), 0, 0);
}
/** This is the const version of start<int>().*/
template<typename Derived>
template<int Size>
inline const typename BlockReturnType<Derived,Size>::SubVectorType
MatrixBase<Derived>::start() const
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return Block<Derived, (RowsAtCompileTime == 1 ? 1 : Size),
(ColsAtCompileTime == 1 ? 1 : Size)>(derived(), 0, 0);
}
/** \returns a fixed-size expression of the last coefficients of *this.
*
* \only_for_vectors
*
* The template parameter \a Size is the number of coefficients in the block
*
* Example: \include MatrixBase_template_int_end.cpp
* Output: \verbinclude MatrixBase_template_int_end.out
*
* \sa class Block
*/
template<typename Derived>
template<int Size>
inline typename BlockReturnType<Derived,Size>::SubVectorType
MatrixBase<Derived>::end()
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return Block<Derived, RowsAtCompileTime == 1 ? 1 : Size,
ColsAtCompileTime == 1 ? 1 : Size>
(derived(),
RowsAtCompileTime == 1 ? 0 : rows() - Size,
ColsAtCompileTime == 1 ? 0 : cols() - Size);
}
/** This is the const version of end<int>.*/
template<typename Derived>
template<int Size>
inline const typename BlockReturnType<Derived,Size>::SubVectorType
MatrixBase<Derived>::end() const
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return Block<Derived, RowsAtCompileTime == 1 ? 1 : Size,
ColsAtCompileTime == 1 ? 1 : Size>
(derived(),
RowsAtCompileTime == 1 ? 0 : rows() - Size,
ColsAtCompileTime == 1 ? 0 : cols() - Size);
}
/** \returns a dynamic-size expression of a corner of *this.
*
* \param type the type of corner. Can be \a Eigen::TopLeft, \a Eigen::TopRight,
* \a Eigen::BottomLeft, \a Eigen::BottomRight.
* \param cRows the number of rows in the corner
* \param cCols the number of columns in the corner
*
* \addexample BlockCornerDynamicSize \label How to reference a sub-corner of a matrix
*
* Example: \include MatrixBase_corner_enum_int_int.cpp
* Output: \verbinclude MatrixBase_corner_enum_int_int.out
*
* \note Even though the returned expression has dynamic size, in the case
* when it is applied to a fixed-size matrix, it inherits a fixed maximal size,
* which means that evaluating it does not cause a dynamic memory allocation.
*
* \sa class Block, block(int,int,int,int)
*/
template<typename Derived>
inline typename BlockReturnType<Derived>::Type MatrixBase<Derived>
::corner(CornerType type, int cRows, int cCols)
{
switch(type)
{
default:
ei_assert(false && "Bad corner type.");
case TopLeft:
return typename BlockReturnType<Derived>::Type(derived(), 0, 0, cRows, cCols);
case TopRight:
return typename BlockReturnType<Derived>::Type(derived(), 0, cols() - cCols, cRows, cCols);
case BottomLeft:
return typename BlockReturnType<Derived>::Type(derived(), rows() - cRows, 0, cRows, cCols);
case BottomRight:
return typename BlockReturnType<Derived>::Type(derived(), rows() - cRows, cols() - cCols, cRows, cCols);
}
}
/** This is the const version of corner(CornerType, int, int).*/
template<typename Derived>
inline const typename BlockReturnType<Derived>::Type
MatrixBase<Derived>::corner(CornerType type, int cRows, int cCols) const
{
switch(type)
{
default:
ei_assert(false && "Bad corner type.");
case TopLeft:
return typename BlockReturnType<Derived>::Type(derived(), 0, 0, cRows, cCols);
case TopRight:
return typename BlockReturnType<Derived>::Type(derived(), 0, cols() - cCols, cRows, cCols);
case BottomLeft:
return typename BlockReturnType<Derived>::Type(derived(), rows() - cRows, 0, cRows, cCols);
case BottomRight:
return typename BlockReturnType<Derived>::Type(derived(), rows() - cRows, cols() - cCols, cRows, cCols);
}
}
/** \returns a fixed-size expression of a corner of *this.
*
* \param type the type of corner. Can be \a Eigen::TopLeft, \a Eigen::TopRight,
* \a Eigen::BottomLeft, \a Eigen::BottomRight.
*
* The template parameters CRows and CCols arethe number of rows and columns in the corner.
*
* Example: \include MatrixBase_template_int_int_corner_enum.cpp
* Output: \verbinclude MatrixBase_template_int_int_corner_enum.out
*
* \sa class Block, block(int,int,int,int)
*/
template<typename Derived>
template<int CRows, int CCols>
inline typename BlockReturnType<Derived, CRows, CCols>::Type
MatrixBase<Derived>::corner(CornerType type)
{
switch(type)
{
default:
ei_assert(false && "Bad corner type.");
case TopLeft:
return Block<Derived, CRows, CCols>(derived(), 0, 0);
case TopRight:
return Block<Derived, CRows, CCols>(derived(), 0, cols() - CCols);
case BottomLeft:
return Block<Derived, CRows, CCols>(derived(), rows() - CRows, 0);
case BottomRight:
return Block<Derived, CRows, CCols>(derived(), rows() - CRows, cols() - CCols);
}
}
/** This is the const version of corner<int, int>(CornerType).*/
template<typename Derived>
template<int CRows, int CCols>
inline const typename BlockReturnType<Derived, CRows, CCols>::Type
MatrixBase<Derived>::corner(CornerType type) const
{
switch(type)
{
default:
ei_assert(false && "Bad corner type.");
case TopLeft:
return Block<Derived, CRows, CCols>(derived(), 0, 0);
case TopRight:
return Block<Derived, CRows, CCols>(derived(), 0, cols() - CCols);
case BottomLeft:
return Block<Derived, CRows, CCols>(derived(), rows() - CRows, 0);
case BottomRight:
return Block<Derived, CRows, CCols>(derived(), rows() - CRows, cols() - CCols);
}
}
/** \returns a fixed-size expression of a block in *this.
*
* The template parameters \a BlockRows and \a BlockCols are the number of
* rows and columns in the block.
*
* \param startRow the first row in the block
* \param startCol the first column in the block
*
* \addexample BlockSubMatrixFixedSize \label How to reference a sub-matrix (fixed-size)
*
* Example: \include MatrixBase_block_int_int.cpp
* Output: \verbinclude MatrixBase_block_int_int.out
*
* \note since block is a templated member, the keyword template has to be used
* if the matrix type is also a template parameter: \code m.template block<3,3>(1,1); \endcode
*
* \sa class Block, block(int,int,int,int)
*/
template<typename Derived>
template<int BlockRows, int BlockCols>
inline typename BlockReturnType<Derived, BlockRows, BlockCols>::Type
MatrixBase<Derived>::block(int startRow, int startCol)
{
return Block<Derived, BlockRows, BlockCols>(derived(), startRow, startCol);
}
/** This is the const version of block<>(int, int). */
template<typename Derived>
template<int BlockRows, int BlockCols>
inline const typename BlockReturnType<Derived, BlockRows, BlockCols>::Type
MatrixBase<Derived>::block(int startRow, int startCol) const
{
return Block<Derived, BlockRows, BlockCols>(derived(), startRow, startCol);
}
/** \returns an expression of the \a i-th column of *this. Note that the numbering starts at 0.
*
* \addexample BlockColumn \label How to reference a single column of a matrix
*
* Example: \include MatrixBase_col.cpp
* Output: \verbinclude MatrixBase_col.out
*
* \sa row(), class Block */
template<typename Derived>
inline typename MatrixBase<Derived>::ColXpr
MatrixBase<Derived>::col(int i)
{
return ColXpr(derived(), i);
}
/** This is the const version of col(). */
template<typename Derived>
inline const typename MatrixBase<Derived>::ColXpr
MatrixBase<Derived>::col(int i) const
{
return ColXpr(derived(), i);
}
/** \returns an expression of the \a i-th row of *this. Note that the numbering starts at 0.
*
* \addexample BlockRow \label How to reference a single row of a matrix
*
* Example: \include MatrixBase_row.cpp
* Output: \verbinclude MatrixBase_row.out
*
* \sa col(), class Block */
template<typename Derived>
inline typename MatrixBase<Derived>::RowXpr
MatrixBase<Derived>::row(int i)
{
return RowXpr(derived(), i);
}
/** This is the const version of row(). */
template<typename Derived>
inline const typename MatrixBase<Derived>::RowXpr
MatrixBase<Derived>::row(int i) const
{
return RowXpr(derived(), i);
}
#endif // EIGEN_BLOCK_H

9
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/CMakeLists.txt Normal file
View File

@ -0,0 +1,9 @@
FILE(GLOB Eigen_Core_SRCS "*.h")
INSTALL(FILES
${Eigen_Core_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Core
)
ADD_SUBDIRECTORY(util)
ADD_SUBDIRECTORY(arch)

753
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/CacheFriendlyProduct.h Normal file
View File

@ -0,0 +1,753 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_CACHE_FRIENDLY_PRODUCT_H
#define EIGEN_CACHE_FRIENDLY_PRODUCT_H
template <int L2MemorySize,typename Scalar>
struct ei_L2_block_traits {
enum {width = 8 * ei_meta_sqrt<L2MemorySize/(64*sizeof(Scalar))>::ret };
};
#ifndef EIGEN_EXTERN_INSTANTIATIONS
template<typename Scalar>
void ei_cache_friendly_product(
int _rows, int _cols, int depth,
bool _lhsRowMajor, const Scalar* _lhs, int _lhsStride,
bool _rhsRowMajor, const Scalar* _rhs, int _rhsStride,
bool resRowMajor, Scalar* res, int resStride)
{
const Scalar* EIGEN_RESTRICT lhs;
const Scalar* EIGEN_RESTRICT rhs;
int lhsStride, rhsStride, rows, cols;
bool lhsRowMajor;
if (resRowMajor)
{
lhs = _rhs;
rhs = _lhs;
lhsStride = _rhsStride;
rhsStride = _lhsStride;
cols = _rows;
rows = _cols;
lhsRowMajor = !_rhsRowMajor;
ei_assert(_lhsRowMajor);
}
else
{
lhs = _lhs;
rhs = _rhs;
lhsStride = _lhsStride;
rhsStride = _rhsStride;
rows = _rows;
cols = _cols;
lhsRowMajor = _lhsRowMajor;
ei_assert(!_rhsRowMajor);
}
typedef typename ei_packet_traits<Scalar>::type PacketType;
enum {
PacketSize = sizeof(PacketType)/sizeof(Scalar),
#if (defined __i386__)
// i386 architecture provides only 8 xmm registers,
// so let's reduce the max number of rows processed at once.
MaxBlockRows = 4,
MaxBlockRows_ClampingMask = 0xFFFFFC,
#else
MaxBlockRows = 8,
MaxBlockRows_ClampingMask = 0xFFFFF8,
#endif
// maximal size of the blocks fitted in L2 cache
MaxL2BlockSize = ei_L2_block_traits<EIGEN_TUNE_FOR_CPU_CACHE_SIZE,Scalar>::width
};
const bool resIsAligned = (PacketSize==1) || (((resStride%PacketSize) == 0) && (std::size_t(res)%16==0));
const int remainingSize = depth % PacketSize;
const int size = depth - remainingSize; // third dimension of the product clamped to packet boundaries
const int l2BlockRows = MaxL2BlockSize > rows ? rows : MaxL2BlockSize;
const int l2BlockCols = MaxL2BlockSize > cols ? cols : MaxL2BlockSize;
const int l2BlockSize = MaxL2BlockSize > size ? size : MaxL2BlockSize;
const int l2BlockSizeAligned = (1 + std::max(l2BlockSize,l2BlockCols)/PacketSize)*PacketSize;
const bool needRhsCopy = (PacketSize>1) && ((rhsStride%PacketSize!=0) || (std::size_t(rhs)%16!=0));
Scalar* EIGEN_RESTRICT block = 0;
const int allocBlockSize = l2BlockRows*size;
block = ei_aligned_stack_new(Scalar, allocBlockSize);
Scalar* EIGEN_RESTRICT rhsCopy
= ei_aligned_stack_new(Scalar, l2BlockSizeAligned*l2BlockSizeAligned);
// loops on each L2 cache friendly blocks of the result
for(int l2i=0; l2i<rows; l2i+=l2BlockRows)
{
const int l2blockRowEnd = std::min(l2i+l2BlockRows, rows);
const int l2blockRowEndBW = l2blockRowEnd & MaxBlockRows_ClampingMask; // end of the rows aligned to bw
const int l2blockRemainingRows = l2blockRowEnd - l2blockRowEndBW; // number of remaining rows
//const int l2blockRowEndBWPlusOne = l2blockRowEndBW + (l2blockRemainingRows?0:MaxBlockRows);
// build a cache friendly blocky matrix
int count = 0;
// copy l2blocksize rows of m_lhs to blocks of ps x bw
for(int l2k=0; l2k<size; l2k+=l2BlockSize)
{
const int l2blockSizeEnd = std::min(l2k+l2BlockSize, size);
for (int i = l2i; i<l2blockRowEndBW/*PlusOne*/; i+=MaxBlockRows)
{
// TODO merge the "if l2blockRemainingRows" using something like:
// const int blockRows = std::min(i+MaxBlockRows, rows) - i;
for (int k=l2k; k<l2blockSizeEnd; k+=PacketSize)
{
// TODO write these loops using meta unrolling
// negligible for large matrices but useful for small ones
if (lhsRowMajor)
{
for (int w=0; w<MaxBlockRows; ++w)
for (int s=0; s<PacketSize; ++s)
block[count++] = lhs[(i+w)*lhsStride + (k+s)];
}
else
{
for (int w=0; w<MaxBlockRows; ++w)
for (int s=0; s<PacketSize; ++s)
block[count++] = lhs[(i+w) + (k+s)*lhsStride];
}
}
}
if (l2blockRemainingRows>0)
{
for (int k=l2k; k<l2blockSizeEnd; k+=PacketSize)
{
if (lhsRowMajor)
{
for (int w=0; w<l2blockRemainingRows; ++w)
for (int s=0; s<PacketSize; ++s)
block[count++] = lhs[(l2blockRowEndBW+w)*lhsStride + (k+s)];
}
else
{
for (int w=0; w<l2blockRemainingRows; ++w)
for (int s=0; s<PacketSize; ++s)
block[count++] = lhs[(l2blockRowEndBW+w) + (k+s)*lhsStride];
}
}
}
}
for(int l2j=0; l2j<cols; l2j+=l2BlockCols)
{
int l2blockColEnd = std::min(l2j+l2BlockCols, cols);
for(int l2k=0; l2k<size; l2k+=l2BlockSize)
{
// acumulate bw rows of lhs time a single column of rhs to a bw x 1 block of res
int l2blockSizeEnd = std::min(l2k+l2BlockSize, size);
// if not aligned, copy the rhs block
if (needRhsCopy)
for(int l1j=l2j; l1j<l2blockColEnd; l1j+=1)
{
ei_internal_assert(l2BlockSizeAligned*(l1j-l2j)+(l2blockSizeEnd-l2k) < l2BlockSizeAligned*l2BlockSizeAligned);
std::memcpy(rhsCopy+l2BlockSizeAligned*(l1j-l2j),&(rhs[l1j*rhsStride+l2k]),(l2blockSizeEnd-l2k)*sizeof(Scalar));
}
// for each bw x 1 result's block
for(int l1i=l2i; l1i<l2blockRowEndBW; l1i+=MaxBlockRows)
{
int offsetblock = l2k * (l2blockRowEnd-l2i) + (l1i-l2i)*(l2blockSizeEnd-l2k) - l2k*MaxBlockRows;
const Scalar* EIGEN_RESTRICT localB = &block[offsetblock];
for(int l1j=l2j; l1j<l2blockColEnd; l1j+=1)
{
const Scalar* EIGEN_RESTRICT rhsColumn;
if (needRhsCopy)
rhsColumn = &(rhsCopy[l2BlockSizeAligned*(l1j-l2j)-l2k]);
else
rhsColumn = &(rhs[l1j*rhsStride]);
PacketType dst[MaxBlockRows];
dst[3] = dst[2] = dst[1] = dst[0] = ei_pset1(Scalar(0.));
if (MaxBlockRows==8)
dst[7] = dst[6] = dst[5] = dst[4] = dst[0];
PacketType tmp;
for(int k=l2k; k<l2blockSizeEnd; k+=PacketSize)
{
tmp = ei_ploadu(&rhsColumn[k]);
PacketType A0, A1, A2, A3, A4, A5;
A0 = ei_pload(localB + k*MaxBlockRows);
A1 = ei_pload(localB + k*MaxBlockRows+1*PacketSize);
A2 = ei_pload(localB + k*MaxBlockRows+2*PacketSize);
A3 = ei_pload(localB + k*MaxBlockRows+3*PacketSize);
if (MaxBlockRows==8) A4 = ei_pload(localB + k*MaxBlockRows+4*PacketSize);
if (MaxBlockRows==8) A5 = ei_pload(localB + k*MaxBlockRows+5*PacketSize);
dst[0] = ei_pmadd(tmp, A0, dst[0]);
if (MaxBlockRows==8) A0 = ei_pload(localB + k*MaxBlockRows+6*PacketSize);
dst[1] = ei_pmadd(tmp, A1, dst[1]);
if (MaxBlockRows==8) A1 = ei_pload(localB + k*MaxBlockRows+7*PacketSize);
dst[2] = ei_pmadd(tmp, A2, dst[2]);
dst[3] = ei_pmadd(tmp, A3, dst[3]);
if (MaxBlockRows==8)
{
dst[4] = ei_pmadd(tmp, A4, dst[4]);
dst[5] = ei_pmadd(tmp, A5, dst[5]);
dst[6] = ei_pmadd(tmp, A0, dst[6]);
dst[7] = ei_pmadd(tmp, A1, dst[7]);
}
}
Scalar* EIGEN_RESTRICT localRes = &(res[l1i + l1j*resStride]);
if (PacketSize>1 && resIsAligned)
{
// the result is aligned: let's do packet reduction
ei_pstore(&(localRes[0]), ei_padd(ei_pload(&(localRes[0])), ei_preduxp(&dst[0])));
if (PacketSize==2)
ei_pstore(&(localRes[2]), ei_padd(ei_pload(&(localRes[2])), ei_preduxp(&(dst[2]))));
if (MaxBlockRows==8)
{
ei_pstore(&(localRes[4]), ei_padd(ei_pload(&(localRes[4])), ei_preduxp(&(dst[4]))));
if (PacketSize==2)
ei_pstore(&(localRes[6]), ei_padd(ei_pload(&(localRes[6])), ei_preduxp(&(dst[6]))));
}
}
else
{
// not aligned => per coeff packet reduction
localRes[0] += ei_predux(dst[0]);
localRes[1] += ei_predux(dst[1]);
localRes[2] += ei_predux(dst[2]);
localRes[3] += ei_predux(dst[3]);
if (MaxBlockRows==8)
{
localRes[4] += ei_predux(dst[4]);
localRes[5] += ei_predux(dst[5]);
localRes[6] += ei_predux(dst[6]);
localRes[7] += ei_predux(dst[7]);
}
}
}
}
if (l2blockRemainingRows>0)
{
int offsetblock = l2k * (l2blockRowEnd-l2i) + (l2blockRowEndBW-l2i)*(l2blockSizeEnd-l2k) - l2k*l2blockRemainingRows;
const Scalar* localB = &block[offsetblock];
for(int l1j=l2j; l1j<l2blockColEnd; l1j+=1)
{
const Scalar* EIGEN_RESTRICT rhsColumn;
if (needRhsCopy)
rhsColumn = &(rhsCopy[l2BlockSizeAligned*(l1j-l2j)-l2k]);
else
rhsColumn = &(rhs[l1j*rhsStride]);
PacketType dst[MaxBlockRows];
dst[3] = dst[2] = dst[1] = dst[0] = ei_pset1(Scalar(0.));
if (MaxBlockRows==8)
dst[7] = dst[6] = dst[5] = dst[4] = dst[0];
// let's declare a few other temporary registers
PacketType tmp;
for(int k=l2k; k<l2blockSizeEnd; k+=PacketSize)
{
tmp = ei_pload(&rhsColumn[k]);
dst[0] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows ])), dst[0]);
if (l2blockRemainingRows>=2) dst[1] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+ PacketSize])), dst[1]);
if (l2blockRemainingRows>=3) dst[2] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+2*PacketSize])), dst[2]);
if (l2blockRemainingRows>=4) dst[3] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+3*PacketSize])), dst[3]);
if (MaxBlockRows==8)
{
if (l2blockRemainingRows>=5) dst[4] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+4*PacketSize])), dst[4]);
if (l2blockRemainingRows>=6) dst[5] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+5*PacketSize])), dst[5]);
if (l2blockRemainingRows>=7) dst[6] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+6*PacketSize])), dst[6]);
if (l2blockRemainingRows>=8) dst[7] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+7*PacketSize])), dst[7]);
}
}
Scalar* EIGEN_RESTRICT localRes = &(res[l2blockRowEndBW + l1j*resStride]);
// process the remaining rows once at a time
localRes[0] += ei_predux(dst[0]);
if (l2blockRemainingRows>=2) localRes[1] += ei_predux(dst[1]);
if (l2blockRemainingRows>=3) localRes[2] += ei_predux(dst[2]);
if (l2blockRemainingRows>=4) localRes[3] += ei_predux(dst[3]);
if (MaxBlockRows==8)
{
if (l2blockRemainingRows>=5) localRes[4] += ei_predux(dst[4]);
if (l2blockRemainingRows>=6) localRes[5] += ei_predux(dst[5]);
if (l2blockRemainingRows>=7) localRes[6] += ei_predux(dst[6]);
if (l2blockRemainingRows>=8) localRes[7] += ei_predux(dst[7]);
}
}
}
}
}
}
if (PacketSize>1 && remainingSize)
{
if (lhsRowMajor)
{
for (int j=0; j<cols; ++j)
for (int i=0; i<rows; ++i)
{
Scalar tmp = lhs[i*lhsStride+size] * rhs[j*rhsStride+size];
// FIXME this loop get vectorized by the compiler !
for (int k=1; k<remainingSize; ++k)
tmp += lhs[i*lhsStride+size+k] * rhs[j*rhsStride+size+k];
res[i+j*resStride] += tmp;
}
}
else
{
for (int j=0; j<cols; ++j)
for (int i=0; i<rows; ++i)
{
Scalar tmp = lhs[i+size*lhsStride] * rhs[j*rhsStride+size];
for (int k=1; k<remainingSize; ++k)
tmp += lhs[i+(size+k)*lhsStride] * rhs[j*rhsStride+size+k];
res[i+j*resStride] += tmp;
}
}
}
ei_aligned_stack_delete(Scalar, block, allocBlockSize);
ei_aligned_stack_delete(Scalar, rhsCopy, l2BlockSizeAligned*l2BlockSizeAligned);
}
#endif // EIGEN_EXTERN_INSTANTIATIONS
/* Optimized col-major matrix * vector product:
* This algorithm processes 4 columns at onces that allows to both reduce
* the number of load/stores of the result by a factor 4 and to reduce
* the instruction dependency. Moreover, we know that all bands have the
* same alignment pattern.
* TODO: since rhs gets evaluated only once, no need to evaluate it
*/
template<typename Scalar, typename RhsType>
EIGEN_DONT_INLINE void ei_cache_friendly_product_colmajor_times_vector(
int size,
const Scalar* lhs, int lhsStride,
const RhsType& rhs,
Scalar* res)
{
#ifdef _EIGEN_ACCUMULATE_PACKETS
#error _EIGEN_ACCUMULATE_PACKETS has already been defined
#endif
#define _EIGEN_ACCUMULATE_PACKETS(A0,A13,A2) \
ei_pstore(&res[j], \
ei_padd(ei_pload(&res[j]), \
ei_padd( \
ei_padd(ei_pmul(ptmp0,EIGEN_CAT(ei_ploa , A0)(&lhs0[j])), \
ei_pmul(ptmp1,EIGEN_CAT(ei_ploa , A13)(&lhs1[j]))), \
ei_padd(ei_pmul(ptmp2,EIGEN_CAT(ei_ploa , A2)(&lhs2[j])), \
ei_pmul(ptmp3,EIGEN_CAT(ei_ploa , A13)(&lhs3[j]))) )))
typedef typename ei_packet_traits<Scalar>::type Packet;
const int PacketSize = sizeof(Packet)/sizeof(Scalar);
enum { AllAligned = 0, EvenAligned, FirstAligned, NoneAligned };
const int columnsAtOnce = 4;
const int peels = 2;
const int PacketAlignedMask = PacketSize-1;
const int PeelAlignedMask = PacketSize*peels-1;
// How many coeffs of the result do we have to skip to be aligned.
// Here we assume data are at least aligned on the base scalar type that is mandatory anyway.
const int alignedStart = ei_alignmentOffset(res,size);
const int alignedSize = PacketSize>1 ? alignedStart + ((size-alignedStart) & ~PacketAlignedMask) : 0;
const int peeledSize = peels>1 ? alignedStart + ((alignedSize-alignedStart) & ~PeelAlignedMask) : alignedStart;
const int alignmentStep = PacketSize>1 ? (PacketSize - lhsStride % PacketSize) & PacketAlignedMask : 0;
int alignmentPattern = alignmentStep==0 ? AllAligned
: alignmentStep==(PacketSize/2) ? EvenAligned
: FirstAligned;
// we cannot assume the first element is aligned because of sub-matrices
const int lhsAlignmentOffset = ei_alignmentOffset(lhs,size);
// find how many columns do we have to skip to be aligned with the result (if possible)
int skipColumns = 0;
if (PacketSize>1)
{
ei_internal_assert(std::size_t(lhs+lhsAlignmentOffset)%sizeof(Packet)==0 || size<PacketSize);
while (skipColumns<PacketSize &&
alignedStart != ((lhsAlignmentOffset + alignmentStep*skipColumns)%PacketSize))
++skipColumns;
if (skipColumns==PacketSize)
{
// nothing can be aligned, no need to skip any column
alignmentPattern = NoneAligned;
skipColumns = 0;
}
else
{
skipColumns = std::min(skipColumns,rhs.size());
// note that the skiped columns are processed later.
}
ei_internal_assert((alignmentPattern==NoneAligned) || (std::size_t(lhs+alignedStart+lhsStride*skipColumns)%sizeof(Packet))==0);
}
int offset1 = (FirstAligned && alignmentStep==1?3:1);
int offset3 = (FirstAligned && alignmentStep==1?1:3);
int columnBound = ((rhs.size()-skipColumns)/columnsAtOnce)*columnsAtOnce + skipColumns;
for (int i=skipColumns; i<columnBound; i+=columnsAtOnce)
{
Packet ptmp0 = ei_pset1(rhs[i]), ptmp1 = ei_pset1(rhs[i+offset1]),
ptmp2 = ei_pset1(rhs[i+2]), ptmp3 = ei_pset1(rhs[i+offset3]);
// this helps a lot generating better binary code
const Scalar *lhs0 = lhs + i*lhsStride, *lhs1 = lhs + (i+offset1)*lhsStride,
*lhs2 = lhs + (i+2)*lhsStride, *lhs3 = lhs + (i+offset3)*lhsStride;
if (PacketSize>1)
{
/* explicit vectorization */
// process initial unaligned coeffs
for (int j=0; j<alignedStart; ++j)
res[j] += ei_pfirst(ptmp0)*lhs0[j] + ei_pfirst(ptmp1)*lhs1[j] + ei_pfirst(ptmp2)*lhs2[j] + ei_pfirst(ptmp3)*lhs3[j];
if (alignedSize>alignedStart)
{
switch(alignmentPattern)
{
case AllAligned:
for (int j = alignedStart; j<alignedSize; j+=PacketSize)
_EIGEN_ACCUMULATE_PACKETS(d,d,d);
break;
case EvenAligned:
for (int j = alignedStart; j<alignedSize; j+=PacketSize)
_EIGEN_ACCUMULATE_PACKETS(d,du,d);
break;
case FirstAligned:
if(peels>1)
{
Packet A00, A01, A02, A03, A10, A11, A12, A13;
A01 = ei_pload(&lhs1[alignedStart-1]);
A02 = ei_pload(&lhs2[alignedStart-2]);
A03 = ei_pload(&lhs3[alignedStart-3]);
for (int j = alignedStart; j<peeledSize; j+=peels*PacketSize)
{
A11 = ei_pload(&lhs1[j-1+PacketSize]); ei_palign<1>(A01,A11);
A12 = ei_pload(&lhs2[j-2+PacketSize]); ei_palign<2>(A02,A12);
A13 = ei_pload(&lhs3[j-3+PacketSize]); ei_palign<3>(A03,A13);
A00 = ei_pload (&lhs0[j]);
A10 = ei_pload (&lhs0[j+PacketSize]);
A00 = ei_pmadd(ptmp0, A00, ei_pload(&res[j]));
A10 = ei_pmadd(ptmp0, A10, ei_pload(&res[j+PacketSize]));
A00 = ei_pmadd(ptmp1, A01, A00);
A01 = ei_pload(&lhs1[j-1+2*PacketSize]); ei_palign<1>(A11,A01);
A00 = ei_pmadd(ptmp2, A02, A00);
A02 = ei_pload(&lhs2[j-2+2*PacketSize]); ei_palign<2>(A12,A02);
A00 = ei_pmadd(ptmp3, A03, A00);
ei_pstore(&res[j],A00);
A03 = ei_pload(&lhs3[j-3+2*PacketSize]); ei_palign<3>(A13,A03);
A10 = ei_pmadd(ptmp1, A11, A10);
A10 = ei_pmadd(ptmp2, A12, A10);
A10 = ei_pmadd(ptmp3, A13, A10);
ei_pstore(&res[j+PacketSize],A10);
}
}
for (int j = peeledSize; j<alignedSize; j+=PacketSize)
_EIGEN_ACCUMULATE_PACKETS(d,du,du);
break;
default:
for (int j = alignedStart; j<alignedSize; j+=PacketSize)
_EIGEN_ACCUMULATE_PACKETS(du,du,du);
break;
}
}
} // end explicit vectorization
/* process remaining coeffs (or all if there is no explicit vectorization) */
for (int j=alignedSize; j<size; ++j)
res[j] += ei_pfirst(ptmp0)*lhs0[j] + ei_pfirst(ptmp1)*lhs1[j] + ei_pfirst(ptmp2)*lhs2[j] + ei_pfirst(ptmp3)*lhs3[j];
}
// process remaining first and last columns (at most columnsAtOnce-1)
int end = rhs.size();
int start = columnBound;
do
{
for (int i=start; i<end; ++i)
{
Packet ptmp0 = ei_pset1(rhs[i]);
const Scalar* lhs0 = lhs + i*lhsStride;
if (PacketSize>1)
{
/* explicit vectorization */
// process first unaligned result's coeffs
for (int j=0; j<alignedStart; ++j)
res[j] += ei_pfirst(ptmp0) * lhs0[j];
// process aligned result's coeffs
if ((std::size_t(lhs0+alignedStart)%sizeof(Packet))==0)
for (int j = alignedStart;j<alignedSize;j+=PacketSize)
ei_pstore(&res[j], ei_pmadd(ptmp0,ei_pload(&lhs0[j]),ei_pload(&res[j])));
else
for (int j = alignedStart;j<alignedSize;j+=PacketSize)
ei_pstore(&res[j], ei_pmadd(ptmp0,ei_ploadu(&lhs0[j]),ei_pload(&res[j])));
}
// process remaining scalars (or all if no explicit vectorization)
for (int j=alignedSize; j<size; ++j)
res[j] += ei_pfirst(ptmp0) * lhs0[j];
}
if (skipColumns)
{
start = 0;
end = skipColumns;
skipColumns = 0;
}
else
break;
} while(PacketSize>1);
#undef _EIGEN_ACCUMULATE_PACKETS
}
// TODO add peeling to mask unaligned load/stores
template<typename Scalar, typename ResType>
EIGEN_DONT_INLINE void ei_cache_friendly_product_rowmajor_times_vector(
const Scalar* lhs, int lhsStride,
const Scalar* rhs, int rhsSize,
ResType& res)
{
#ifdef _EIGEN_ACCUMULATE_PACKETS
#error _EIGEN_ACCUMULATE_PACKETS has already been defined
#endif
#define _EIGEN_ACCUMULATE_PACKETS(A0,A13,A2) {\
Packet b = ei_pload(&rhs[j]); \
ptmp0 = ei_pmadd(b, EIGEN_CAT(ei_ploa,A0) (&lhs0[j]), ptmp0); \
ptmp1 = ei_pmadd(b, EIGEN_CAT(ei_ploa,A13)(&lhs1[j]), ptmp1); \
ptmp2 = ei_pmadd(b, EIGEN_CAT(ei_ploa,A2) (&lhs2[j]), ptmp2); \
ptmp3 = ei_pmadd(b, EIGEN_CAT(ei_ploa,A13)(&lhs3[j]), ptmp3); }
typedef typename ei_packet_traits<Scalar>::type Packet;
const int PacketSize = sizeof(Packet)/sizeof(Scalar);
enum { AllAligned=0, EvenAligned=1, FirstAligned=2, NoneAligned=3 };
const int rowsAtOnce = 4;
const int peels = 2;
const int PacketAlignedMask = PacketSize-1;
const int PeelAlignedMask = PacketSize*peels-1;
const int size = rhsSize;
// How many coeffs of the result do we have to skip to be aligned.
// Here we assume data are at least aligned on the base scalar type that is mandatory anyway.
const int alignedStart = ei_alignmentOffset(rhs, size);
const int alignedSize = PacketSize>1 ? alignedStart + ((size-alignedStart) & ~PacketAlignedMask) : 0;
const int peeledSize = peels>1 ? alignedStart + ((alignedSize-alignedStart) & ~PeelAlignedMask) : alignedStart;
const int alignmentStep = PacketSize>1 ? (PacketSize - lhsStride % PacketSize) & PacketAlignedMask : 0;
int alignmentPattern = alignmentStep==0 ? AllAligned
: alignmentStep==(PacketSize/2) ? EvenAligned
: FirstAligned;
// we cannot assume the first element is aligned because of sub-matrices
const int lhsAlignmentOffset = ei_alignmentOffset(lhs,size);
// find how many rows do we have to skip to be aligned with rhs (if possible)
int skipRows = 0;
if (PacketSize>1)
{
ei_internal_assert(std::size_t(lhs+lhsAlignmentOffset)%sizeof(Packet)==0 || size<PacketSize);
while (skipRows<PacketSize &&
alignedStart != ((lhsAlignmentOffset + alignmentStep*skipRows)%PacketSize))
++skipRows;
if (skipRows==PacketSize)
{
// nothing can be aligned, no need to skip any column
alignmentPattern = NoneAligned;
skipRows = 0;
}
else
{
skipRows = std::min(skipRows,res.size());
// note that the skiped columns are processed later.
}
ei_internal_assert((alignmentPattern==NoneAligned) || PacketSize==1
|| (std::size_t(lhs+alignedStart+lhsStride*skipRows)%sizeof(Packet))==0);
}
int offset1 = (FirstAligned && alignmentStep==1?3:1);
int offset3 = (FirstAligned && alignmentStep==1?1:3);
int rowBound = ((res.size()-skipRows)/rowsAtOnce)*rowsAtOnce + skipRows;
for (int i=skipRows; i<rowBound; i+=rowsAtOnce)
{
Scalar tmp0 = Scalar(0), tmp1 = Scalar(0), tmp2 = Scalar(0), tmp3 = Scalar(0);
// this helps the compiler generating good binary code
const Scalar *lhs0 = lhs + i*lhsStride, *lhs1 = lhs + (i+offset1)*lhsStride,
*lhs2 = lhs + (i+2)*lhsStride, *lhs3 = lhs + (i+offset3)*lhsStride;
if (PacketSize>1)
{
/* explicit vectorization */
Packet ptmp0 = ei_pset1(Scalar(0)), ptmp1 = ei_pset1(Scalar(0)), ptmp2 = ei_pset1(Scalar(0)), ptmp3 = ei_pset1(Scalar(0));
// process initial unaligned coeffs
// FIXME this loop get vectorized by the compiler !
for (int j=0; j<alignedStart; ++j)
{
Scalar b = rhs[j];
tmp0 += b*lhs0[j]; tmp1 += b*lhs1[j]; tmp2 += b*lhs2[j]; tmp3 += b*lhs3[j];
}
if (alignedSize>alignedStart)
{
switch(alignmentPattern)
{
case AllAligned:
for (int j = alignedStart; j<alignedSize; j+=PacketSize)
_EIGEN_ACCUMULATE_PACKETS(d,d,d);
break;
case EvenAligned:
for (int j = alignedStart; j<alignedSize; j+=PacketSize)
_EIGEN_ACCUMULATE_PACKETS(d,du,d);
break;
case FirstAligned:
if (peels>1)
{
/* Here we proccess 4 rows with with two peeled iterations to hide
* tghe overhead of unaligned loads. Moreover unaligned loads are handled
* using special shift/move operations between the two aligned packets
* overlaping the desired unaligned packet. This is *much* more efficient
* than basic unaligned loads.
*/
Packet A01, A02, A03, b, A11, A12, A13;
A01 = ei_pload(&lhs1[alignedStart-1]);
A02 = ei_pload(&lhs2[alignedStart-2]);
A03 = ei_pload(&lhs3[alignedStart-3]);
for (int j = alignedStart; j<peeledSize; j+=peels*PacketSize)
{
b = ei_pload(&rhs[j]);
A11 = ei_pload(&lhs1[j-1+PacketSize]); ei_palign<1>(A01,A11);
A12 = ei_pload(&lhs2[j-2+PacketSize]); ei_palign<2>(A02,A12);
A13 = ei_pload(&lhs3[j-3+PacketSize]); ei_palign<3>(A03,A13);
ptmp0 = ei_pmadd(b, ei_pload (&lhs0[j]), ptmp0);
ptmp1 = ei_pmadd(b, A01, ptmp1);
A01 = ei_pload(&lhs1[j-1+2*PacketSize]); ei_palign<1>(A11,A01);
ptmp2 = ei_pmadd(b, A02, ptmp2);
A02 = ei_pload(&lhs2[j-2+2*PacketSize]); ei_palign<2>(A12,A02);
ptmp3 = ei_pmadd(b, A03, ptmp3);
A03 = ei_pload(&lhs3[j-3+2*PacketSize]); ei_palign<3>(A13,A03);
b = ei_pload(&rhs[j+PacketSize]);
ptmp0 = ei_pmadd(b, ei_pload (&lhs0[j+PacketSize]), ptmp0);
ptmp1 = ei_pmadd(b, A11, ptmp1);
ptmp2 = ei_pmadd(b, A12, ptmp2);
ptmp3 = ei_pmadd(b, A13, ptmp3);
}
}
for (int j = peeledSize; j<alignedSize; j+=PacketSize)
_EIGEN_ACCUMULATE_PACKETS(d,du,du);
break;
default:
for (int j = alignedStart; j<alignedSize; j+=PacketSize)
_EIGEN_ACCUMULATE_PACKETS(du,du,du);
break;
}
tmp0 += ei_predux(ptmp0);
tmp1 += ei_predux(ptmp1);
tmp2 += ei_predux(ptmp2);
tmp3 += ei_predux(ptmp3);
}
} // end explicit vectorization
// process remaining coeffs (or all if no explicit vectorization)
// FIXME this loop get vectorized by the compiler !
for (int j=alignedSize; j<size; ++j)
{
Scalar b = rhs[j];
tmp0 += b*lhs0[j]; tmp1 += b*lhs1[j]; tmp2 += b*lhs2[j]; tmp3 += b*lhs3[j];
}
res[i] += tmp0; res[i+offset1] += tmp1; res[i+2] += tmp2; res[i+offset3] += tmp3;
}
// process remaining first and last rows (at most columnsAtOnce-1)
int end = res.size();
int start = rowBound;
do
{
for (int i=start; i<end; ++i)
{
Scalar tmp0 = Scalar(0);
Packet ptmp0 = ei_pset1(tmp0);
const Scalar* lhs0 = lhs + i*lhsStride;
// process first unaligned result's coeffs
// FIXME this loop get vectorized by the compiler !
for (int j=0; j<alignedStart; ++j)
tmp0 += rhs[j] * lhs0[j];
if (alignedSize>alignedStart)
{
// process aligned rhs coeffs
if ((std::size_t(lhs0+alignedStart)%sizeof(Packet))==0)
for (int j = alignedStart;j<alignedSize;j+=PacketSize)
ptmp0 = ei_pmadd(ei_pload(&rhs[j]), ei_pload(&lhs0[j]), ptmp0);
else
for (int j = alignedStart;j<alignedSize;j+=PacketSize)
ptmp0 = ei_pmadd(ei_pload(&rhs[j]), ei_ploadu(&lhs0[j]), ptmp0);
tmp0 += ei_predux(ptmp0);
}
// process remaining scalars
// FIXME this loop get vectorized by the compiler !
for (int j=alignedSize; j<size; ++j)
tmp0 += rhs[j] * lhs0[j];
res[i] += tmp0;
}
if (skipRows)
{
start = 0;
end = skipRows;
skipRows = 0;
}
else
break;
} while(PacketSize>1);
#undef _EIGEN_ACCUMULATE_PACKETS
}
#endif // EIGEN_CACHE_FRIENDLY_PRODUCT_H

384
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Coeffs.h Normal file
View File

@ -0,0 +1,384 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_COEFFS_H
#define EIGEN_COEFFS_H
/** Short version: don't use this function, use
* \link operator()(int,int) const \endlink instead.
*
* Long version: this function is similar to
* \link operator()(int,int) const \endlink, but without the assertion.
* Use this for limiting the performance cost of debugging code when doing
* repeated coefficient access. Only use this when it is guaranteed that the
* parameters \a row and \a col are in range.
*
* If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this
* function equivalent to \link operator()(int,int) const \endlink.
*
* \sa operator()(int,int) const, coeffRef(int,int), coeff(int) const
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
::coeff(int row, int col) const
{
ei_internal_assert(row >= 0 && row < rows()
&& col >= 0 && col < cols());
return derived().coeff(row, col);
}
/** \returns the coefficient at given the given row and column.
*
* \sa operator()(int,int), operator[](int) const
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
::operator()(int row, int col) const
{
ei_assert(row >= 0 && row < rows()
&& col >= 0 && col < cols());
return derived().coeff(row, col);
}
/** Short version: don't use this function, use
* \link operator()(int,int) \endlink instead.
*
* Long version: this function is similar to
* \link operator()(int,int) \endlink, but without the assertion.
* Use this for limiting the performance cost of debugging code when doing
* repeated coefficient access. Only use this when it is guaranteed that the
* parameters \a row and \a col are in range.
*
* If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this
* function equivalent to \link operator()(int,int) \endlink.
*
* \sa operator()(int,int), coeff(int, int) const, coeffRef(int)
*/
template<typename Derived>
EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
::coeffRef(int row, int col)
{
ei_internal_assert(row >= 0 && row < rows()
&& col >= 0 && col < cols());
return derived().coeffRef(row, col);
}
/** \returns a reference to the coefficient at given the given row and column.
*
* \sa operator()(int,int) const, operator[](int)
*/
template<typename Derived>
EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
::operator()(int row, int col)
{
ei_assert(row >= 0 && row < rows()
&& col >= 0 && col < cols());
return derived().coeffRef(row, col);
}
/** Short version: don't use this function, use
* \link operator[](int) const \endlink instead.
*
* Long version: this function is similar to
* \link operator[](int) const \endlink, but without the assertion.
* Use this for limiting the performance cost of debugging code when doing
* repeated coefficient access. Only use this when it is guaranteed that the
* parameter \a index is in range.
*
* If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this
* function equivalent to \link operator[](int) const \endlink.
*
* \sa operator[](int) const, coeffRef(int), coeff(int,int) const
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
::coeff(int index) const
{
ei_internal_assert(index >= 0 && index < size());
return derived().coeff(index);
}
/** \returns the coefficient at given index.
*
* This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit.
*
* \sa operator[](int), operator()(int,int) const, x() const, y() const,
* z() const, w() const
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
::operator[](int index) const
{
ei_assert(index >= 0 && index < size());
return derived().coeff(index);
}
/** \returns the coefficient at given index.
*
* This is synonymous to operator[](int) const.
*
* This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit.
*
* \sa operator[](int), operator()(int,int) const, x() const, y() const,
* z() const, w() const
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
::operator()(int index) const
{
ei_assert(index >= 0 && index < size());
return derived().coeff(index);
}
/** Short version: don't use this function, use
* \link operator[](int) \endlink instead.
*
* Long version: this function is similar to
* \link operator[](int) \endlink, but without the assertion.
* Use this for limiting the performance cost of debugging code when doing
* repeated coefficient access. Only use this when it is guaranteed that the
* parameters \a row and \a col are in range.
*
* If EIGEN_INTERNAL_DEBUGGING is defined, an assertion will be made, making this
* function equivalent to \link operator[](int) \endlink.
*
* \sa operator[](int), coeff(int) const, coeffRef(int,int)
*/
template<typename Derived>
EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
::coeffRef(int index)
{
ei_internal_assert(index >= 0 && index < size());
return derived().coeffRef(index);
}
/** \returns a reference to the coefficient at given index.
*
* This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit.
*
* \sa operator[](int) const, operator()(int,int), x(), y(), z(), w()
*/
template<typename Derived>
EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
::operator[](int index)
{
ei_assert(index >= 0 && index < size());
return derived().coeffRef(index);
}
/** \returns a reference to the coefficient at given index.
*
* This is synonymous to operator[](int).
*
* This method is allowed only for vector expressions, and for matrix expressions having the LinearAccessBit.
*
* \sa operator[](int) const, operator()(int,int), x(), y(), z(), w()
*/
template<typename Derived>
EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
::operator()(int index)
{
ei_assert(index >= 0 && index < size());
return derived().coeffRef(index);
}
/** equivalent to operator[](0). */
template<typename Derived>
EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
::x() const { return (*this)[0]; }
/** equivalent to operator[](1). */
template<typename Derived>
EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
::y() const { return (*this)[1]; }
/** equivalent to operator[](2). */
template<typename Derived>
EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
::z() const { return (*this)[2]; }
/** equivalent to operator[](3). */
template<typename Derived>
EIGEN_STRONG_INLINE const typename ei_traits<Derived>::Scalar MatrixBase<Derived>
::w() const { return (*this)[3]; }
/** equivalent to operator[](0). */
template<typename Derived>
EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
::x() { return (*this)[0]; }
/** equivalent to operator[](1). */
template<typename Derived>
EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
::y() { return (*this)[1]; }
/** equivalent to operator[](2). */
template<typename Derived>
EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
::z() { return (*this)[2]; }
/** equivalent to operator[](3). */
template<typename Derived>
EIGEN_STRONG_INLINE typename ei_traits<Derived>::Scalar& MatrixBase<Derived>
::w() { return (*this)[3]; }
/** \returns the packet of coefficients starting at the given row and column. It is your responsibility
* to ensure that a packet really starts there. This method is only available on expressions having the
* PacketAccessBit.
*
* The \a LoadMode parameter may have the value \a Aligned or \a Unaligned. Its effect is to select
* the appropriate vectorization instruction. Aligned access is faster, but is only possible for packets
* starting at an address which is a multiple of the packet size.
*/
template<typename Derived>
template<int LoadMode>
EIGEN_STRONG_INLINE typename ei_packet_traits<typename ei_traits<Derived>::Scalar>::type
MatrixBase<Derived>::packet(int row, int col) const
{
ei_internal_assert(row >= 0 && row < rows()
&& col >= 0 && col < cols());
return derived().template packet<LoadMode>(row,col);
}
/** Stores the given packet of coefficients, at the given row and column of this expression. It is your responsibility
* to ensure that a packet really starts there. This method is only available on expressions having the
* PacketAccessBit.
*
* The \a LoadMode parameter may have the value \a Aligned or \a Unaligned. Its effect is to select
* the appropriate vectorization instruction. Aligned access is faster, but is only possible for packets
* starting at an address which is a multiple of the packet size.
*/
template<typename Derived>
template<int StoreMode>
EIGEN_STRONG_INLINE void MatrixBase<Derived>::writePacket
(int row, int col, const typename ei_packet_traits<typename ei_traits<Derived>::Scalar>::type& x)
{
ei_internal_assert(row >= 0 && row < rows()
&& col >= 0 && col < cols());
derived().template writePacket<StoreMode>(row,col,x);
}
/** \returns the packet of coefficients starting at the given index. It is your responsibility
* to ensure that a packet really starts there. This method is only available on expressions having the
* PacketAccessBit and the LinearAccessBit.
*
* The \a LoadMode parameter may have the value \a Aligned or \a Unaligned. Its effect is to select
* the appropriate vectorization instruction. Aligned access is faster, but is only possible for packets
* starting at an address which is a multiple of the packet size.
*/
template<typename Derived>
template<int LoadMode>
EIGEN_STRONG_INLINE typename ei_packet_traits<typename ei_traits<Derived>::Scalar>::type
MatrixBase<Derived>::packet(int index) const
{
ei_internal_assert(index >= 0 && index < size());
return derived().template packet<LoadMode>(index);
}
/** Stores the given packet of coefficients, at the given index in this expression. It is your responsibility
* to ensure that a packet really starts there. This method is only available on expressions having the
* PacketAccessBit and the LinearAccessBit.
*
* The \a LoadMode parameter may have the value \a Aligned or \a Unaligned. Its effect is to select
* the appropriate vectorization instruction. Aligned access is faster, but is only possible for packets
* starting at an address which is a multiple of the packet size.
*/
template<typename Derived>
template<int StoreMode>
EIGEN_STRONG_INLINE void MatrixBase<Derived>::writePacket
(int index, const typename ei_packet_traits<typename ei_traits<Derived>::Scalar>::type& x)
{
ei_internal_assert(index >= 0 && index < size());
derived().template writePacket<StoreMode>(index,x);
}
#ifndef EIGEN_PARSED_BY_DOXYGEN
/** \internal Copies the coefficient at position (row,col) of other into *this.
*
* This method is overridden in SwapWrapper, allowing swap() assignments to share 99% of their code
* with usual assignments.
*
* Outside of this internal usage, this method has probably no usefulness. It is hidden in the public API dox.
*/
template<typename Derived>
template<typename OtherDerived>
EIGEN_STRONG_INLINE void MatrixBase<Derived>::copyCoeff(int row, int col, const MatrixBase<OtherDerived>& other)
{
ei_internal_assert(row >= 0 && row < rows()
&& col >= 0 && col < cols());
derived().coeffRef(row, col) = other.derived().coeff(row, col);
}
/** \internal Copies the coefficient at the given index of other into *this.
*
* This method is overridden in SwapWrapper, allowing swap() assignments to share 99% of their code
* with usual assignments.
*
* Outside of this internal usage, this method has probably no usefulness. It is hidden in the public API dox.
*/
template<typename Derived>
template<typename OtherDerived>
EIGEN_STRONG_INLINE void MatrixBase<Derived>::copyCoeff(int index, const MatrixBase<OtherDerived>& other)
{
ei_internal_assert(index >= 0 && index < size());
derived().coeffRef(index) = other.derived().coeff(index);
}
/** \internal Copies the packet at position (row,col) of other into *this.
*
* This method is overridden in SwapWrapper, allowing swap() assignments to share 99% of their code
* with usual assignments.
*
* Outside of this internal usage, this method has probably no usefulness. It is hidden in the public API dox.
*/
template<typename Derived>
template<typename OtherDerived, int StoreMode, int LoadMode>
EIGEN_STRONG_INLINE void MatrixBase<Derived>::copyPacket(int row, int col, const MatrixBase<OtherDerived>& other)
{
ei_internal_assert(row >= 0 && row < rows()
&& col >= 0 && col < cols());
derived().template writePacket<StoreMode>(row, col,
other.derived().template packet<LoadMode>(row, col));
}
/** \internal Copies the packet at the given index of other into *this.
*
* This method is overridden in SwapWrapper, allowing swap() assignments to share 99% of their code
* with usual assignments.
*
* Outside of this internal usage, this method has probably no usefulness. It is hidden in the public API dox.
*/
template<typename Derived>
template<typename OtherDerived, int StoreMode, int LoadMode>
EIGEN_STRONG_INLINE void MatrixBase<Derived>::copyPacket(int index, const MatrixBase<OtherDerived>& other)
{
ei_internal_assert(index >= 0 && index < size());
derived().template writePacket<StoreMode>(index,
other.derived().template packet<LoadMode>(index));
}
#endif
#endif // EIGEN_COEFFS_H

152
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/CommaInitializer.h Normal file
View File

@ -0,0 +1,152 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_COMMAINITIALIZER_H
#define EIGEN_COMMAINITIALIZER_H
/** \class CommaInitializer
*
* \brief Helper class used by the comma initializer operator
*
* This class is internally used to implement the comma initializer feature. It is
* the return type of MatrixBase::operator<<, and most of the time this is the only
* way it is used.
*
* \sa \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished()
*/
template<typename MatrixType>
struct CommaInitializer
{
typedef typename ei_traits<MatrixType>::Scalar Scalar;
inline CommaInitializer(MatrixType& mat, const Scalar& s)
: m_matrix(mat), m_row(0), m_col(1), m_currentBlockRows(1)
{
m_matrix.coeffRef(0,0) = s;
}
template<typename OtherDerived>
inline CommaInitializer(MatrixType& mat, const MatrixBase<OtherDerived>& other)
: m_matrix(mat), m_row(0), m_col(other.cols()), m_currentBlockRows(other.rows())
{
m_matrix.block(0, 0, other.rows(), other.cols()) = other;
}
/* inserts a scalar value in the target matrix */
CommaInitializer& operator,(const Scalar& s)
{
if (m_col==m_matrix.cols())
{
m_row+=m_currentBlockRows;
m_col = 0;
m_currentBlockRows = 1;
ei_assert(m_row<m_matrix.rows()
&& "Too many rows passed to comma initializer (operator<<)");
}
ei_assert(m_col<m_matrix.cols()
&& "Too many coefficients passed to comma initializer (operator<<)");
ei_assert(m_currentBlockRows==1);
m_matrix.coeffRef(m_row, m_col++) = s;
return *this;
}
/* inserts a matrix expression in the target matrix */
template<typename OtherDerived>
CommaInitializer& operator,(const MatrixBase<OtherDerived>& other)
{
if (m_col==m_matrix.cols())
{
m_row+=m_currentBlockRows;
m_col = 0;
m_currentBlockRows = other.rows();
ei_assert(m_row+m_currentBlockRows<=m_matrix.rows()
&& "Too many rows passed to comma initializer (operator<<)");
}
ei_assert(m_col<m_matrix.cols()
&& "Too many coefficients passed to comma initializer (operator<<)");
ei_assert(m_currentBlockRows==other.rows());
if (OtherDerived::SizeAtCompileTime != Dynamic)
m_matrix.template block<OtherDerived::RowsAtCompileTime != Dynamic ? OtherDerived::RowsAtCompileTime : 1,
OtherDerived::ColsAtCompileTime != Dynamic ? OtherDerived::ColsAtCompileTime : 1>
(m_row, m_col) = other;
else
m_matrix.block(m_row, m_col, other.rows(), other.cols()) = other;
m_col += other.cols();
return *this;
}
inline ~CommaInitializer()
{
ei_assert((m_row+m_currentBlockRows) == m_matrix.rows()
&& m_col == m_matrix.cols()
&& "Too few coefficients passed to comma initializer (operator<<)");
}
/** \returns the built matrix once all its coefficients have been set.
* Calling finished is 100% optional. Its purpose is to write expressions
* like this:
* \code
* quaternion.fromRotationMatrix((Matrix3f() << axis0, axis1, axis2).finished());
* \endcode
*/
inline MatrixType& finished() { return m_matrix; }
MatrixType& m_matrix; // target matrix
int m_row; // current row id
int m_col; // current col id
int m_currentBlockRows; // current block height
private:
CommaInitializer& operator=(const CommaInitializer&);
};
/** \anchor MatrixBaseCommaInitRef
* Convenient operator to set the coefficients of a matrix.
*
* The coefficients must be provided in a row major order and exactly match
* the size of the matrix. Otherwise an assertion is raised.
*
* \addexample CommaInit \label How to easily set all the coefficients of a matrix
*
* Example: \include MatrixBase_set.cpp
* Output: \verbinclude MatrixBase_set.out
*
* \sa CommaInitializer::finished(), class CommaInitializer
*/
template<typename Derived>
inline CommaInitializer<Derived> MatrixBase<Derived>::operator<< (const Scalar& s)
{
return CommaInitializer<Derived>(*static_cast<Derived*>(this), s);
}
/** \sa operator<<(const Scalar&) */
template<typename Derived>
template<typename OtherDerived>
inline CommaInitializer<Derived>
MatrixBase<Derived>::operator<<(const MatrixBase<OtherDerived>& other)
{
return CommaInitializer<Derived>(*static_cast<Derived *>(this), other);
}
#endif // EIGEN_COMMAINITIALIZER_H

47
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/CoreInstantiations.cpp Normal file
View File

@ -0,0 +1,47 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifdef EIGEN_EXTERN_INSTANTIATIONS
#undef EIGEN_EXTERN_INSTANTIATIONS
#endif
#include "../../Core"
namespace Eigen
{
#define EIGEN_INSTANTIATE_PRODUCT(TYPE) \
template void ei_cache_friendly_product<TYPE>( \
int _rows, int _cols, int depth, \
bool _lhsRowMajor, const TYPE* _lhs, int _lhsStride, \
bool _rhsRowMajor, const TYPE* _rhs, int _rhsStride, \
bool resRowMajor, TYPE* res, int resStride)
EIGEN_INSTANTIATE_PRODUCT(float);
EIGEN_INSTANTIATE_PRODUCT(double);
EIGEN_INSTANTIATE_PRODUCT(int);
EIGEN_INSTANTIATE_PRODUCT(std::complex<float>);
EIGEN_INSTANTIATE_PRODUCT(std::complex<double>);
}

214
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Cwise.h Normal file
View File

@ -0,0 +1,214 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_CWISE_H
#define EIGEN_CWISE_H
/** \internal
* convenient macro to defined the return type of a cwise binary operation */
#define EIGEN_CWISE_BINOP_RETURN_TYPE(OP) \
CwiseBinaryOp<OP<typename ei_traits<ExpressionType>::Scalar>, ExpressionType, OtherDerived>
#define EIGEN_CWISE_PRODUCT_RETURN_TYPE \
CwiseBinaryOp< \
ei_scalar_product_op< \
typename ei_scalar_product_traits< \
typename ei_traits<ExpressionType>::Scalar, \
typename ei_traits<OtherDerived>::Scalar \
>::ReturnType \
>, \
ExpressionType, \
OtherDerived \
>
/** \internal
* convenient macro to defined the return type of a cwise unary operation */
#define EIGEN_CWISE_UNOP_RETURN_TYPE(OP) \
CwiseUnaryOp<OP<typename ei_traits<ExpressionType>::Scalar>, ExpressionType>
/** \internal
* convenient macro to defined the return type of a cwise comparison to a scalar */
#define EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(OP) \
CwiseBinaryOp<OP<typename ei_traits<ExpressionType>::Scalar>, ExpressionType, \
NestByValue<typename ExpressionType::ConstantReturnType> >
/** \class Cwise
*
* \brief Pseudo expression providing additional coefficient-wise operations
*
* \param ExpressionType the type of the object on which to do coefficient-wise operations
*
* This class represents an expression with additional coefficient-wise features.
* It is the return type of MatrixBase::cwise()
* and most of the time this is the only way it is used.
*
* Note that some methods are defined in the \ref Array module.
*
* Example: \include MatrixBase_cwise_const.cpp
* Output: \verbinclude MatrixBase_cwise_const.out
*
* \sa MatrixBase::cwise() const, MatrixBase::cwise()
*/
template<typename ExpressionType> class Cwise
{
public:
typedef typename ei_traits<ExpressionType>::Scalar Scalar;
typedef typename ei_meta_if<ei_must_nest_by_value<ExpressionType>::ret,
ExpressionType, const ExpressionType&>::ret ExpressionTypeNested;
typedef CwiseUnaryOp<ei_scalar_add_op<Scalar>, ExpressionType> ScalarAddReturnType;
inline Cwise(const ExpressionType& matrix) : m_matrix(matrix) {}
/** \internal */
inline const ExpressionType& _expression() const { return m_matrix; }
template<typename OtherDerived>
const EIGEN_CWISE_PRODUCT_RETURN_TYPE
operator*(const MatrixBase<OtherDerived> &other) const;
template<typename OtherDerived>
const EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_quotient_op)
operator/(const MatrixBase<OtherDerived> &other) const;
template<typename OtherDerived>
const EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_min_op)
min(const MatrixBase<OtherDerived> &other) const;
template<typename OtherDerived>
const EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_max_op)
max(const MatrixBase<OtherDerived> &other) const;
const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_abs_op) abs() const;
const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_abs2_op) abs2() const;
const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_square_op) square() const;
const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_cube_op) cube() const;
const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_inverse_op) inverse() const;
const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_sqrt_op) sqrt() const;
const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_exp_op) exp() const;
const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_log_op) log() const;
const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_cos_op) cos() const;
const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_sin_op) sin() const;
const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_pow_op) pow(const Scalar& exponent) const;
const ScalarAddReturnType
operator+(const Scalar& scalar) const;
/** \relates Cwise */
friend const ScalarAddReturnType
operator+(const Scalar& scalar, const Cwise& mat)
{ return mat + scalar; }
ExpressionType& operator+=(const Scalar& scalar);
const ScalarAddReturnType
operator-(const Scalar& scalar) const;
ExpressionType& operator-=(const Scalar& scalar);
template<typename OtherDerived>
inline ExpressionType& operator*=(const MatrixBase<OtherDerived> &other);
template<typename OtherDerived>
inline ExpressionType& operator/=(const MatrixBase<OtherDerived> &other);
template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::less)
operator<(const MatrixBase<OtherDerived>& other) const;
template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::less_equal)
operator<=(const MatrixBase<OtherDerived>& other) const;
template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater)
operator>(const MatrixBase<OtherDerived>& other) const;
template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::greater_equal)
operator>=(const MatrixBase<OtherDerived>& other) const;
template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::equal_to)
operator==(const MatrixBase<OtherDerived>& other) const;
template<typename OtherDerived> const EIGEN_CWISE_BINOP_RETURN_TYPE(std::not_equal_to)
operator!=(const MatrixBase<OtherDerived>& other) const;
// comparisons to a scalar value
const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less)
operator<(Scalar s) const;
const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::less_equal)
operator<=(Scalar s) const;
const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater)
operator>(Scalar s) const;
const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::greater_equal)
operator>=(Scalar s) const;
const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::equal_to)
operator==(Scalar s) const;
const EIGEN_CWISE_COMP_TO_SCALAR_RETURN_TYPE(std::not_equal_to)
operator!=(Scalar s) const;
// allow to extend Cwise outside Eigen
#ifdef EIGEN_CWISE_PLUGIN
#include EIGEN_CWISE_PLUGIN
#endif
protected:
ExpressionTypeNested m_matrix;
private:
Cwise& operator=(const Cwise&);
};
/** \returns a Cwise wrapper of *this providing additional coefficient-wise operations
*
* Example: \include MatrixBase_cwise_const.cpp
* Output: \verbinclude MatrixBase_cwise_const.out
*
* \sa class Cwise, cwise()
*/
template<typename Derived>
inline const Cwise<Derived>
MatrixBase<Derived>::cwise() const
{
return derived();
}
/** \returns a Cwise wrapper of *this providing additional coefficient-wise operations
*
* Example: \include MatrixBase_cwise.cpp
* Output: \verbinclude MatrixBase_cwise.out
*
* \sa class Cwise, cwise() const
*/
template<typename Derived>
inline Cwise<Derived>
MatrixBase<Derived>::cwise()
{
return derived();
}
#endif // EIGEN_CWISE_H

304
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/CwiseBinaryOp.h Normal file
View File

@ -0,0 +1,304 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_CWISE_BINARY_OP_H
#define EIGEN_CWISE_BINARY_OP_H
/** \class CwiseBinaryOp
*
* \brief Generic expression of a coefficient-wise operator between two matrices or vectors
*
* \param BinaryOp template functor implementing the operator
* \param Lhs the type of the left-hand side
* \param Rhs the type of the right-hand side
*
* This class represents an expression of a generic binary operator of two matrices or vectors.
* It is the return type of the operator+, operator-, and the Cwise methods, and most
* of the time this is the only way it is used.
*
* However, if you want to write a function returning such an expression, you
* will need to use this class.
*
* \sa MatrixBase::binaryExpr(const MatrixBase<OtherDerived> &,const CustomBinaryOp &) const, class CwiseUnaryOp, class CwiseNullaryOp
*/
template<typename BinaryOp, typename Lhs, typename Rhs>
struct ei_traits<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
{
// even though we require Lhs and Rhs to have the same scalar type (see CwiseBinaryOp constructor),
// we still want to handle the case when the result type is different.
typedef typename ei_result_of<
BinaryOp(
typename Lhs::Scalar,
typename Rhs::Scalar
)
>::type Scalar;
typedef typename Lhs::Nested LhsNested;
typedef typename Rhs::Nested RhsNested;
typedef typename ei_unref<LhsNested>::type _LhsNested;
typedef typename ei_unref<RhsNested>::type _RhsNested;
enum {
LhsCoeffReadCost = _LhsNested::CoeffReadCost,
RhsCoeffReadCost = _RhsNested::CoeffReadCost,
LhsFlags = _LhsNested::Flags,
RhsFlags = _RhsNested::Flags,
RowsAtCompileTime = Lhs::RowsAtCompileTime,
ColsAtCompileTime = Lhs::ColsAtCompileTime,
MaxRowsAtCompileTime = Lhs::MaxRowsAtCompileTime,
MaxColsAtCompileTime = Lhs::MaxColsAtCompileTime,
Flags = (int(LhsFlags) | int(RhsFlags)) & (
HereditaryBits
| (int(LhsFlags) & int(RhsFlags) & (LinearAccessBit | AlignedBit))
| (ei_functor_traits<BinaryOp>::PacketAccess && ((int(LhsFlags) & RowMajorBit)==(int(RhsFlags) & RowMajorBit))
? (int(LhsFlags) & int(RhsFlags) & PacketAccessBit) : 0)),
CoeffReadCost = LhsCoeffReadCost + RhsCoeffReadCost + ei_functor_traits<BinaryOp>::Cost
};
};
template<typename BinaryOp, typename Lhs, typename Rhs>
class CwiseBinaryOp : ei_no_assignment_operator,
public MatrixBase<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
{
public:
EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseBinaryOp)
typedef typename ei_traits<CwiseBinaryOp>::LhsNested LhsNested;
typedef typename ei_traits<CwiseBinaryOp>::RhsNested RhsNested;
EIGEN_STRONG_INLINE CwiseBinaryOp(const Lhs& lhs, const Rhs& rhs, const BinaryOp& func = BinaryOp())
: m_lhs(lhs), m_rhs(rhs), m_functor(func)
{
// we require Lhs and Rhs to have the same scalar type. Currently there is no example of a binary functor
// that would take two operands of different types. If there were such an example, then this check should be
// moved to the BinaryOp functors, on a per-case basis. This would however require a change in the BinaryOp functors, as
// currently they take only one typename Scalar template parameter.
// It is tempting to always allow mixing different types but remember that this is often impossible in the vectorized paths.
// So allowing mixing different types gives very unexpected errors when enabling vectorization, when the user tries to
// add together a float matrix and a double matrix.
EIGEN_STATIC_ASSERT((ei_functor_allows_mixing_real_and_complex<BinaryOp>::ret
? int(ei_is_same_type<typename Lhs::RealScalar, typename Rhs::RealScalar>::ret)
: int(ei_is_same_type<typename Lhs::Scalar, typename Rhs::Scalar>::ret)),
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
// require the sizes to match
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Lhs, Rhs)
ei_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols());
}
EIGEN_STRONG_INLINE int rows() const { return m_lhs.rows(); }
EIGEN_STRONG_INLINE int cols() const { return m_lhs.cols(); }
EIGEN_STRONG_INLINE const Scalar coeff(int row, int col) const
{
return m_functor(m_lhs.coeff(row, col), m_rhs.coeff(row, col));
}
template<int LoadMode>
EIGEN_STRONG_INLINE PacketScalar packet(int row, int col) const
{
return m_functor.packetOp(m_lhs.template packet<LoadMode>(row, col), m_rhs.template packet<LoadMode>(row, col));
}
EIGEN_STRONG_INLINE const Scalar coeff(int index) const
{
return m_functor(m_lhs.coeff(index), m_rhs.coeff(index));
}
template<int LoadMode>
EIGEN_STRONG_INLINE PacketScalar packet(int index) const
{
return m_functor.packetOp(m_lhs.template packet<LoadMode>(index), m_rhs.template packet<LoadMode>(index));
}
protected:
const LhsNested m_lhs;
const RhsNested m_rhs;
const BinaryOp m_functor;
};
/**\returns an expression of the difference of \c *this and \a other
*
* \note If you want to substract a given scalar from all coefficients, see Cwise::operator-().
*
* \sa class CwiseBinaryOp, MatrixBase::operator-=(), Cwise::operator-()
*/
template<typename Derived>
template<typename OtherDerived>
EIGEN_STRONG_INLINE const CwiseBinaryOp<ei_scalar_difference_op<typename ei_traits<Derived>::Scalar>,
Derived, OtherDerived>
MatrixBase<Derived>::operator-(const MatrixBase<OtherDerived> &other) const
{
return CwiseBinaryOp<ei_scalar_difference_op<Scalar>,
Derived, OtherDerived>(derived(), other.derived());
}
/** replaces \c *this by \c *this - \a other.
*
* \returns a reference to \c *this
*/
template<typename Derived>
template<typename OtherDerived>
EIGEN_STRONG_INLINE Derived &
MatrixBase<Derived>::operator-=(const MatrixBase<OtherDerived> &other)
{
return *this = *this - other;
}
/** \relates MatrixBase
*
* \returns an expression of the sum of \c *this and \a other
*
* \note If you want to add a given scalar to all coefficients, see Cwise::operator+().
*
* \sa class CwiseBinaryOp, MatrixBase::operator+=(), Cwise::operator+()
*/
template<typename Derived>
template<typename OtherDerived>
EIGEN_STRONG_INLINE const CwiseBinaryOp<ei_scalar_sum_op<typename ei_traits<Derived>::Scalar>, Derived, OtherDerived>
MatrixBase<Derived>::operator+(const MatrixBase<OtherDerived> &other) const
{
return CwiseBinaryOp<ei_scalar_sum_op<Scalar>, Derived, OtherDerived>(derived(), other.derived());
}
/** replaces \c *this by \c *this + \a other.
*
* \returns a reference to \c *this
*/
template<typename Derived>
template<typename OtherDerived>
EIGEN_STRONG_INLINE Derived &
MatrixBase<Derived>::operator+=(const MatrixBase<OtherDerived>& other)
{
return *this = *this + other;
}
/** \returns an expression of the Schur product (coefficient wise product) of *this and \a other
*
* Example: \include Cwise_product.cpp
* Output: \verbinclude Cwise_product.out
*
* \sa class CwiseBinaryOp, operator/(), square()
*/
template<typename ExpressionType>
template<typename OtherDerived>
EIGEN_STRONG_INLINE const EIGEN_CWISE_PRODUCT_RETURN_TYPE
Cwise<ExpressionType>::operator*(const MatrixBase<OtherDerived> &other) const
{
return EIGEN_CWISE_PRODUCT_RETURN_TYPE(_expression(), other.derived());
}
/** \returns an expression of the coefficient-wise quotient of *this and \a other
*
* Example: \include Cwise_quotient.cpp
* Output: \verbinclude Cwise_quotient.out
*
* \sa class CwiseBinaryOp, operator*(), inverse()
*/
template<typename ExpressionType>
template<typename OtherDerived>
EIGEN_STRONG_INLINE const EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_quotient_op)
Cwise<ExpressionType>::operator/(const MatrixBase<OtherDerived> &other) const
{
return EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_quotient_op)(_expression(), other.derived());
}
/** Replaces this expression by its coefficient-wise product with \a other.
*
* Example: \include Cwise_times_equal.cpp
* Output: \verbinclude Cwise_times_equal.out
*
* \sa operator*(), operator/=()
*/
template<typename ExpressionType>
template<typename OtherDerived>
inline ExpressionType& Cwise<ExpressionType>::operator*=(const MatrixBase<OtherDerived> &other)
{
return m_matrix.const_cast_derived() = *this * other;
}
/** Replaces this expression by its coefficient-wise quotient by \a other.
*
* Example: \include Cwise_slash_equal.cpp
* Output: \verbinclude Cwise_slash_equal.out
*
* \sa operator/(), operator*=()
*/
template<typename ExpressionType>
template<typename OtherDerived>
inline ExpressionType& Cwise<ExpressionType>::operator/=(const MatrixBase<OtherDerived> &other)
{
return m_matrix.const_cast_derived() = *this / other;
}
/** \returns an expression of the coefficient-wise min of *this and \a other
*
* Example: \include Cwise_min.cpp
* Output: \verbinclude Cwise_min.out
*
* \sa class CwiseBinaryOp
*/
template<typename ExpressionType>
template<typename OtherDerived>
EIGEN_STRONG_INLINE const EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_min_op)
Cwise<ExpressionType>::min(const MatrixBase<OtherDerived> &other) const
{
return EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_min_op)(_expression(), other.derived());
}
/** \returns an expression of the coefficient-wise max of *this and \a other
*
* Example: \include Cwise_max.cpp
* Output: \verbinclude Cwise_max.out
*
* \sa class CwiseBinaryOp
*/
template<typename ExpressionType>
template<typename OtherDerived>
EIGEN_STRONG_INLINE const EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_max_op)
Cwise<ExpressionType>::max(const MatrixBase<OtherDerived> &other) const
{
return EIGEN_CWISE_BINOP_RETURN_TYPE(ei_scalar_max_op)(_expression(), other.derived());
}
/** \returns an expression of a custom coefficient-wise operator \a func of *this and \a other
*
* The template parameter \a CustomBinaryOp is the type of the functor
* of the custom operator (see class CwiseBinaryOp for an example)
*
* \addexample CustomCwiseBinaryFunctors \label How to use custom coeff wise binary functors
*
* Here is an example illustrating the use of custom functors:
* \include class_CwiseBinaryOp.cpp
* Output: \verbinclude class_CwiseBinaryOp.out
*
* \sa class CwiseBinaryOp, MatrixBase::operator+, MatrixBase::operator-, Cwise::operator*, Cwise::operator/
*/
template<typename Derived>
template<typename CustomBinaryOp, typename OtherDerived>
EIGEN_STRONG_INLINE const CwiseBinaryOp<CustomBinaryOp, Derived, OtherDerived>
MatrixBase<Derived>::binaryExpr(const MatrixBase<OtherDerived> &other, const CustomBinaryOp& func) const
{
return CwiseBinaryOp<CustomBinaryOp, Derived, OtherDerived>(derived(), other.derived(), func);
}
#endif // EIGEN_CWISE_BINARY_OP_H

763
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/CwiseNullaryOp.h Normal file
View File

@ -0,0 +1,763 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_CWISE_NULLARY_OP_H
#define EIGEN_CWISE_NULLARY_OP_H
/** \class CwiseNullaryOp
*
* \brief Generic expression of a matrix where all coefficients are defined by a functor
*
* \param NullaryOp template functor implementing the operator
*
* This class represents an expression of a generic nullary operator.
* It is the return type of the Ones(), Zero(), Constant(), Identity() and Random() functions,
* and most of the time this is the only way it is used.
*
* However, if you want to write a function returning such an expression, you
* will need to use this class.
*
* \sa class CwiseUnaryOp, class CwiseBinaryOp, MatrixBase::NullaryExpr()
*/
template<typename NullaryOp, typename MatrixType>
struct ei_traits<CwiseNullaryOp<NullaryOp, MatrixType> > : ei_traits<MatrixType>
{
enum {
Flags = (ei_traits<MatrixType>::Flags
& ( HereditaryBits
| (ei_functor_has_linear_access<NullaryOp>::ret ? LinearAccessBit : 0)
| (ei_functor_traits<NullaryOp>::PacketAccess ? PacketAccessBit : 0)))
| (ei_functor_traits<NullaryOp>::IsRepeatable ? 0 : EvalBeforeNestingBit),
CoeffReadCost = ei_functor_traits<NullaryOp>::Cost
};
};
template<typename NullaryOp, typename MatrixType>
class CwiseNullaryOp : ei_no_assignment_operator,
public MatrixBase<CwiseNullaryOp<NullaryOp, MatrixType> >
{
public:
EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseNullaryOp)
CwiseNullaryOp(int rows, int cols, const NullaryOp& func = NullaryOp())
: m_rows(rows), m_cols(cols), m_functor(func)
{
ei_assert(rows > 0
&& (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows)
&& cols > 0
&& (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols));
}
EIGEN_STRONG_INLINE int rows() const { return m_rows.value(); }
EIGEN_STRONG_INLINE int cols() const { return m_cols.value(); }
EIGEN_STRONG_INLINE const Scalar coeff(int rows, int cols) const
{
return m_functor(rows, cols);
}
template<int LoadMode>
EIGEN_STRONG_INLINE PacketScalar packet(int, int) const
{
return m_functor.packetOp();
}
EIGEN_STRONG_INLINE const Scalar coeff(int index) const
{
if(RowsAtCompileTime == 1)
return m_functor(0, index);
else
return m_functor(index, 0);
}
template<int LoadMode>
EIGEN_STRONG_INLINE PacketScalar packet(int) const
{
return m_functor.packetOp();
}
protected:
const ei_int_if_dynamic<RowsAtCompileTime> m_rows;
const ei_int_if_dynamic<ColsAtCompileTime> m_cols;
const NullaryOp m_functor;
};
/** \returns an expression of a matrix defined by a custom functor \a func
*
* The parameters \a rows and \a cols are the number of rows and of columns of
* the returned matrix. Must be compatible with this MatrixBase type.
*
* This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
* it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used
* instead.
*
* The template parameter \a CustomNullaryOp is the type of the functor.
*
* \sa class CwiseNullaryOp
*/
template<typename Derived>
template<typename CustomNullaryOp>
EIGEN_STRONG_INLINE const CwiseNullaryOp<CustomNullaryOp, Derived>
MatrixBase<Derived>::NullaryExpr(int rows, int cols, const CustomNullaryOp& func)
{
return CwiseNullaryOp<CustomNullaryOp, Derived>(rows, cols, func);
}
/** \returns an expression of a matrix defined by a custom functor \a func
*
* The parameter \a size is the size of the returned vector.
* Must be compatible with this MatrixBase type.
*
* \only_for_vectors
*
* This variant is meant to be used for dynamic-size vector types. For fixed-size types,
* it is redundant to pass \a size as argument, so Zero() should be used
* instead.
*
* The template parameter \a CustomNullaryOp is the type of the functor.
*
* \sa class CwiseNullaryOp
*/
template<typename Derived>
template<typename CustomNullaryOp>
EIGEN_STRONG_INLINE const CwiseNullaryOp<CustomNullaryOp, Derived>
MatrixBase<Derived>::NullaryExpr(int size, const CustomNullaryOp& func)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
ei_assert(IsVectorAtCompileTime);
if(RowsAtCompileTime == 1) return CwiseNullaryOp<CustomNullaryOp, Derived>(1, size, func);
else return CwiseNullaryOp<CustomNullaryOp, Derived>(size, 1, func);
}
/** \returns an expression of a matrix defined by a custom functor \a func
*
* This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
* need to use the variants taking size arguments.
*
* The template parameter \a CustomNullaryOp is the type of the functor.
*
* \sa class CwiseNullaryOp
*/
template<typename Derived>
template<typename CustomNullaryOp>
EIGEN_STRONG_INLINE const CwiseNullaryOp<CustomNullaryOp, Derived>
MatrixBase<Derived>::NullaryExpr(const CustomNullaryOp& func)
{
return CwiseNullaryOp<CustomNullaryOp, Derived>(RowsAtCompileTime, ColsAtCompileTime, func);
}
/** \returns an expression of a constant matrix of value \a value
*
* The parameters \a rows and \a cols are the number of rows and of columns of
* the returned matrix. Must be compatible with this MatrixBase type.
*
* This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
* it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used
* instead.
*
* The template parameter \a CustomNullaryOp is the type of the functor.
*
* \sa class CwiseNullaryOp
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
MatrixBase<Derived>::Constant(int rows, int cols, const Scalar& value)
{
return NullaryExpr(rows, cols, ei_scalar_constant_op<Scalar>(value));
}
/** \returns an expression of a constant matrix of value \a value
*
* The parameter \a size is the size of the returned vector.
* Must be compatible with this MatrixBase type.
*
* \only_for_vectors
*
* This variant is meant to be used for dynamic-size vector types. For fixed-size types,
* it is redundant to pass \a size as argument, so Zero() should be used
* instead.
*
* The template parameter \a CustomNullaryOp is the type of the functor.
*
* \sa class CwiseNullaryOp
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
MatrixBase<Derived>::Constant(int size, const Scalar& value)
{
return NullaryExpr(size, ei_scalar_constant_op<Scalar>(value));
}
/** \returns an expression of a constant matrix of value \a value
*
* This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
* need to use the variants taking size arguments.
*
* The template parameter \a CustomNullaryOp is the type of the functor.
*
* \sa class CwiseNullaryOp
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
MatrixBase<Derived>::Constant(const Scalar& value)
{
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
return NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, ei_scalar_constant_op<Scalar>(value));
}
/** \returns true if all coefficients in this matrix are approximately equal to \a value, to within precision \a prec */
template<typename Derived>
bool MatrixBase<Derived>::isApproxToConstant
(const Scalar& value, RealScalar prec) const
{
for(int j = 0; j < cols(); ++j)
for(int i = 0; i < rows(); ++i)
if(!ei_isApprox(coeff(i, j), value, prec))
return false;
return true;
}
/** This is just an alias for isApproxToConstant().
*
* \returns true if all coefficients in this matrix are approximately equal to \a value, to within precision \a prec */
template<typename Derived>
bool MatrixBase<Derived>::isConstant
(const Scalar& value, RealScalar prec) const
{
return isApproxToConstant(value, prec);
}
/** Alias for setConstant(): sets all coefficients in this expression to \a value.
*
* \sa setConstant(), Constant(), class CwiseNullaryOp
*/
template<typename Derived>
EIGEN_STRONG_INLINE void MatrixBase<Derived>::fill(const Scalar& value)
{
setConstant(value);
}
/** Sets all coefficients in this expression to \a value.
*
* \sa fill(), setConstant(int,const Scalar&), setConstant(int,int,const Scalar&), setZero(), setOnes(), Constant(), class CwiseNullaryOp, setZero(), setOnes()
*/
template<typename Derived>
EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setConstant(const Scalar& value)
{
return derived() = Constant(rows(), cols(), value);
}
/** Resizes to the given \a size, and sets all coefficients in this expression to the given \a value.
*
* \only_for_vectors
*
* Example: \include Matrix_set_int.cpp
* Output: \verbinclude Matrix_setConstant_int.out
*
* \sa MatrixBase::setConstant(const Scalar&), setConstant(int,int,const Scalar&), class CwiseNullaryOp, MatrixBase::Constant(const Scalar&)
*/
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setConstant(int size, const Scalar& value)
{
resize(size);
return setConstant(value);
}
/** Resizes to the given size, and sets all coefficients in this expression to the given \a value.
*
* \param rows the new number of rows
* \param cols the new number of columns
*
* Example: \include Matrix_setConstant_int_int.cpp
* Output: \verbinclude Matrix_setConstant_int_int.out
*
* \sa MatrixBase::setConstant(const Scalar&), setConstant(int,const Scalar&), class CwiseNullaryOp, MatrixBase::Constant(const Scalar&)
*/
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setConstant(int rows, int cols, const Scalar& value)
{
resize(rows, cols);
return setConstant(value);
}
// zero:
/** \returns an expression of a zero matrix.
*
* The parameters \a rows and \a cols are the number of rows and of columns of
* the returned matrix. Must be compatible with this MatrixBase type.
*
* This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
* it is redundant to pass \a rows and \a cols as arguments, so Zero() should be used
* instead.
*
* \addexample Zero \label How to take get a zero matrix
*
* Example: \include MatrixBase_zero_int_int.cpp
* Output: \verbinclude MatrixBase_zero_int_int.out
*
* \sa Zero(), Zero(int)
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
MatrixBase<Derived>::Zero(int rows, int cols)
{
return Constant(rows, cols, Scalar(0));
}
/** \returns an expression of a zero vector.
*
* The parameter \a size is the size of the returned vector.
* Must be compatible with this MatrixBase type.
*
* \only_for_vectors
*
* This variant is meant to be used for dynamic-size vector types. For fixed-size types,
* it is redundant to pass \a size as argument, so Zero() should be used
* instead.
*
* Example: \include MatrixBase_zero_int.cpp
* Output: \verbinclude MatrixBase_zero_int.out
*
* \sa Zero(), Zero(int,int)
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
MatrixBase<Derived>::Zero(int size)
{
return Constant(size, Scalar(0));
}
/** \returns an expression of a fixed-size zero matrix or vector.
*
* This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
* need to use the variants taking size arguments.
*
* Example: \include MatrixBase_zero.cpp
* Output: \verbinclude MatrixBase_zero.out
*
* \sa Zero(int), Zero(int,int)
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
MatrixBase<Derived>::Zero()
{
return Constant(Scalar(0));
}
/** \returns true if *this is approximately equal to the zero matrix,
* within the precision given by \a prec.
*
* Example: \include MatrixBase_isZero.cpp
* Output: \verbinclude MatrixBase_isZero.out
*
* \sa class CwiseNullaryOp, Zero()
*/
template<typename Derived>
bool MatrixBase<Derived>::isZero(RealScalar prec) const
{
for(int j = 0; j < cols(); ++j)
for(int i = 0; i < rows(); ++i)
if(!ei_isMuchSmallerThan(coeff(i, j), static_cast<Scalar>(1), prec))
return false;
return true;
}
/** Sets all coefficients in this expression to zero.
*
* Example: \include MatrixBase_setZero.cpp
* Output: \verbinclude MatrixBase_setZero.out
*
* \sa class CwiseNullaryOp, Zero()
*/
template<typename Derived>
EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setZero()
{
return setConstant(Scalar(0));
}
/** Resizes to the given \a size, and sets all coefficients in this expression to zero.
*
* \only_for_vectors
*
* Example: \include Matrix_setZero_int.cpp
* Output: \verbinclude Matrix_setZero_int.out
*
* \sa MatrixBase::setZero(), setZero(int,int), class CwiseNullaryOp, MatrixBase::Zero()
*/
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setZero(int size)
{
resize(size);
return setConstant(Scalar(0));
}
/** Resizes to the given size, and sets all coefficients in this expression to zero.
*
* \param rows the new number of rows
* \param cols the new number of columns
*
* Example: \include Matrix_setZero_int_int.cpp
* Output: \verbinclude Matrix_setZero_int_int.out
*
* \sa MatrixBase::setZero(), setZero(int), class CwiseNullaryOp, MatrixBase::Zero()
*/
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setZero(int rows, int cols)
{
resize(rows, cols);
return setConstant(Scalar(0));
}
// ones:
/** \returns an expression of a matrix where all coefficients equal one.
*
* The parameters \a rows and \a cols are the number of rows and of columns of
* the returned matrix. Must be compatible with this MatrixBase type.
*
* This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
* it is redundant to pass \a rows and \a cols as arguments, so Ones() should be used
* instead.
*
* \addexample One \label How to get a matrix with all coefficients equal one
*
* Example: \include MatrixBase_ones_int_int.cpp
* Output: \verbinclude MatrixBase_ones_int_int.out
*
* \sa Ones(), Ones(int), isOnes(), class Ones
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
MatrixBase<Derived>::Ones(int rows, int cols)
{
return Constant(rows, cols, Scalar(1));
}
/** \returns an expression of a vector where all coefficients equal one.
*
* The parameter \a size is the size of the returned vector.
* Must be compatible with this MatrixBase type.
*
* \only_for_vectors
*
* This variant is meant to be used for dynamic-size vector types. For fixed-size types,
* it is redundant to pass \a size as argument, so Ones() should be used
* instead.
*
* Example: \include MatrixBase_ones_int.cpp
* Output: \verbinclude MatrixBase_ones_int.out
*
* \sa Ones(), Ones(int,int), isOnes(), class Ones
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
MatrixBase<Derived>::Ones(int size)
{
return Constant(size, Scalar(1));
}
/** \returns an expression of a fixed-size matrix or vector where all coefficients equal one.
*
* This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
* need to use the variants taking size arguments.
*
* Example: \include MatrixBase_ones.cpp
* Output: \verbinclude MatrixBase_ones.out
*
* \sa Ones(int), Ones(int,int), isOnes(), class Ones
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ConstantReturnType
MatrixBase<Derived>::Ones()
{
return Constant(Scalar(1));
}
/** \returns true if *this is approximately equal to the matrix where all coefficients
* are equal to 1, within the precision given by \a prec.
*
* Example: \include MatrixBase_isOnes.cpp
* Output: \verbinclude MatrixBase_isOnes.out
*
* \sa class CwiseNullaryOp, Ones()
*/
template<typename Derived>
bool MatrixBase<Derived>::isOnes
(RealScalar prec) const
{
return isApproxToConstant(Scalar(1), prec);
}
/** Sets all coefficients in this expression to one.
*
* Example: \include MatrixBase_setOnes.cpp
* Output: \verbinclude MatrixBase_setOnes.out
*
* \sa class CwiseNullaryOp, Ones()
*/
template<typename Derived>
EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setOnes()
{
return setConstant(Scalar(1));
}
/** Resizes to the given \a size, and sets all coefficients in this expression to one.
*
* \only_for_vectors
*
* Example: \include Matrix_setOnes_int.cpp
* Output: \verbinclude Matrix_setOnes_int.out
*
* \sa MatrixBase::setOnes(), setOnes(int,int), class CwiseNullaryOp, MatrixBase::Ones()
*/
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setOnes(int size)
{
resize(size);
return setConstant(Scalar(1));
}
/** Resizes to the given size, and sets all coefficients in this expression to one.
*
* \param rows the new number of rows
* \param cols the new number of columns
*
* Example: \include Matrix_setOnes_int_int.cpp
* Output: \verbinclude Matrix_setOnes_int_int.out
*
* \sa MatrixBase::setOnes(), setOnes(int), class CwiseNullaryOp, MatrixBase::Ones()
*/
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setOnes(int rows, int cols)
{
resize(rows, cols);
return setConstant(Scalar(1));
}
// Identity:
/** \returns an expression of the identity matrix (not necessarily square).
*
* The parameters \a rows and \a cols are the number of rows and of columns of
* the returned matrix. Must be compatible with this MatrixBase type.
*
* This variant is meant to be used for dynamic-size matrix types. For fixed-size types,
* it is redundant to pass \a rows and \a cols as arguments, so Identity() should be used
* instead.
*
* \addexample Identity \label How to get an identity matrix
*
* Example: \include MatrixBase_identity_int_int.cpp
* Output: \verbinclude MatrixBase_identity_int_int.out
*
* \sa Identity(), setIdentity(), isIdentity()
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::IdentityReturnType
MatrixBase<Derived>::Identity(int rows, int cols)
{
return NullaryExpr(rows, cols, ei_scalar_identity_op<Scalar>());
}
/** \returns an expression of the identity matrix (not necessarily square).
*
* This variant is only for fixed-size MatrixBase types. For dynamic-size types, you
* need to use the variant taking size arguments.
*
* Example: \include MatrixBase_identity.cpp
* Output: \verbinclude MatrixBase_identity.out
*
* \sa Identity(int,int), setIdentity(), isIdentity()
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::IdentityReturnType
MatrixBase<Derived>::Identity()
{
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
return NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, ei_scalar_identity_op<Scalar>());
}
/** \returns true if *this is approximately equal to the identity matrix
* (not necessarily square),
* within the precision given by \a prec.
*
* Example: \include MatrixBase_isIdentity.cpp
* Output: \verbinclude MatrixBase_isIdentity.out
*
* \sa class CwiseNullaryOp, Identity(), Identity(int,int), setIdentity()
*/
template<typename Derived>
bool MatrixBase<Derived>::isIdentity
(RealScalar prec) const
{
for(int j = 0; j < cols(); ++j)
{
for(int i = 0; i < rows(); ++i)
{
if(i == j)
{
if(!ei_isApprox(coeff(i, j), static_cast<Scalar>(1), prec))
return false;
}
else
{
if(!ei_isMuchSmallerThan(coeff(i, j), static_cast<RealScalar>(1), prec))
return false;
}
}
}
return true;
}
template<typename Derived, bool Big = (Derived::SizeAtCompileTime>=16)>
struct ei_setIdentity_impl
{
static EIGEN_STRONG_INLINE Derived& run(Derived& m)
{
return m = Derived::Identity(m.rows(), m.cols());
}
};
template<typename Derived>
struct ei_setIdentity_impl<Derived, true>
{
static EIGEN_STRONG_INLINE Derived& run(Derived& m)
{
m.setZero();
const int size = std::min(m.rows(), m.cols());
for(int i = 0; i < size; ++i) m.coeffRef(i,i) = typename Derived::Scalar(1);
return m;
}
};
/** Writes the identity expression (not necessarily square) into *this.
*
* Example: \include MatrixBase_setIdentity.cpp
* Output: \verbinclude MatrixBase_setIdentity.out
*
* \sa class CwiseNullaryOp, Identity(), Identity(int,int), isIdentity()
*/
template<typename Derived>
EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setIdentity()
{
return ei_setIdentity_impl<Derived>::run(derived());
}
/** Resizes to the given size, and writes the identity expression (not necessarily square) into *this.
*
* \param rows the new number of rows
* \param cols the new number of columns
*
* Example: \include Matrix_setIdentity_int_int.cpp
* Output: \verbinclude Matrix_setIdentity_int_int.out
*
* \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Identity()
*/
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
EIGEN_STRONG_INLINE Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::setIdentity(int rows, int cols)
{
resize(rows, cols);
return setIdentity();
}
/** \returns an expression of the i-th unit (basis) vector.
*
* \only_for_vectors
*
* \sa MatrixBase::Unit(int), MatrixBase::UnitX(), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::Unit(int size, int i)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return BasisReturnType(SquareMatrixType::Identity(size,size), i);
}
/** \returns an expression of the i-th unit (basis) vector.
*
* \only_for_vectors
*
* This variant is for fixed-size vector only.
*
* \sa MatrixBase::Unit(int,int), MatrixBase::UnitX(), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::Unit(int i)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return BasisReturnType(SquareMatrixType::Identity(),i);
}
/** \returns an expression of the X axis unit vector (1{,0}^*)
*
* \only_for_vectors
*
* \sa MatrixBase::Unit(int,int), MatrixBase::Unit(int), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitX()
{ return Derived::Unit(0); }
/** \returns an expression of the Y axis unit vector (0,1{,0}^*)
*
* \only_for_vectors
*
* \sa MatrixBase::Unit(int,int), MatrixBase::Unit(int), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitY()
{ return Derived::Unit(1); }
/** \returns an expression of the Z axis unit vector (0,0,1{,0}^*)
*
* \only_for_vectors
*
* \sa MatrixBase::Unit(int,int), MatrixBase::Unit(int), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitZ()
{ return Derived::Unit(2); }
/** \returns an expression of the W axis unit vector (0,0,0,1)
*
* \only_for_vectors
*
* \sa MatrixBase::Unit(int,int), MatrixBase::Unit(int), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW()
*/
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitW()
{ return Derived::Unit(3); }
#endif // EIGEN_CWISE_NULLARY_OP_H

229
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/CwiseUnaryOp.h Normal file
View File

@ -0,0 +1,229 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_CWISE_UNARY_OP_H
#define EIGEN_CWISE_UNARY_OP_H
/** \class CwiseUnaryOp
*
* \brief Generic expression of a coefficient-wise unary operator of a matrix or a vector
*
* \param UnaryOp template functor implementing the operator
* \param MatrixType the type of the matrix we are applying the unary operator
*
* This class represents an expression of a generic unary operator of a matrix or a vector.
* It is the return type of the unary operator-, of a matrix or a vector, and most
* of the time this is the only way it is used.
*
* \sa MatrixBase::unaryExpr(const CustomUnaryOp &) const, class CwiseBinaryOp, class CwiseNullaryOp
*/
template<typename UnaryOp, typename MatrixType>
struct ei_traits<CwiseUnaryOp<UnaryOp, MatrixType> >
: ei_traits<MatrixType>
{
typedef typename ei_result_of<
UnaryOp(typename MatrixType::Scalar)
>::type Scalar;
typedef typename MatrixType::Nested MatrixTypeNested;
typedef typename ei_unref<MatrixTypeNested>::type _MatrixTypeNested;
enum {
Flags = (_MatrixTypeNested::Flags & (
HereditaryBits | LinearAccessBit | AlignedBit
| (ei_functor_traits<UnaryOp>::PacketAccess ? PacketAccessBit : 0))),
CoeffReadCost = _MatrixTypeNested::CoeffReadCost + ei_functor_traits<UnaryOp>::Cost
};
};
template<typename UnaryOp, typename MatrixType>
class CwiseUnaryOp : ei_no_assignment_operator,
public MatrixBase<CwiseUnaryOp<UnaryOp, MatrixType> >
{
public:
EIGEN_GENERIC_PUBLIC_INTERFACE(CwiseUnaryOp)
inline CwiseUnaryOp(const MatrixType& mat, const UnaryOp& func = UnaryOp())
: m_matrix(mat), m_functor(func) {}
EIGEN_STRONG_INLINE int rows() const { return m_matrix.rows(); }
EIGEN_STRONG_INLINE int cols() const { return m_matrix.cols(); }
EIGEN_STRONG_INLINE const Scalar coeff(int row, int col) const
{
return m_functor(m_matrix.coeff(row, col));
}
template<int LoadMode>
EIGEN_STRONG_INLINE PacketScalar packet(int row, int col) const
{
return m_functor.packetOp(m_matrix.template packet<LoadMode>(row, col));
}
EIGEN_STRONG_INLINE const Scalar coeff(int index) const
{
return m_functor(m_matrix.coeff(index));
}
template<int LoadMode>
EIGEN_STRONG_INLINE PacketScalar packet(int index) const
{
return m_functor.packetOp(m_matrix.template packet<LoadMode>(index));
}
protected:
const typename MatrixType::Nested m_matrix;
const UnaryOp m_functor;
};
/** \returns an expression of a custom coefficient-wise unary operator \a func of *this
*
* The template parameter \a CustomUnaryOp is the type of the functor
* of the custom unary operator.
*
* \addexample CustomCwiseUnaryFunctors \label How to use custom coeff wise unary functors
*
* Example:
* \include class_CwiseUnaryOp.cpp
* Output: \verbinclude class_CwiseUnaryOp.out
*
* \sa class CwiseUnaryOp, class CwiseBinarOp, MatrixBase::operator-, Cwise::abs
*/
template<typename Derived>
template<typename CustomUnaryOp>
EIGEN_STRONG_INLINE const CwiseUnaryOp<CustomUnaryOp, Derived>
MatrixBase<Derived>::unaryExpr(const CustomUnaryOp& func) const
{
return CwiseUnaryOp<CustomUnaryOp, Derived>(derived(), func);
}
/** \returns an expression of the opposite of \c *this
*/
template<typename Derived>
EIGEN_STRONG_INLINE const CwiseUnaryOp<ei_scalar_opposite_op<typename ei_traits<Derived>::Scalar>,Derived>
MatrixBase<Derived>::operator-() const
{
return derived();
}
/** \returns an expression of the coefficient-wise absolute value of \c *this
*
* Example: \include Cwise_abs.cpp
* Output: \verbinclude Cwise_abs.out
*
* \sa abs2()
*/
template<typename ExpressionType>
EIGEN_STRONG_INLINE const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_abs_op)
Cwise<ExpressionType>::abs() const
{
return _expression();
}
/** \returns an expression of the coefficient-wise squared absolute value of \c *this
*
* Example: \include Cwise_abs2.cpp
* Output: \verbinclude Cwise_abs2.out
*
* \sa abs(), square()
*/
template<typename ExpressionType>
EIGEN_STRONG_INLINE const EIGEN_CWISE_UNOP_RETURN_TYPE(ei_scalar_abs2_op)
Cwise<ExpressionType>::abs2() const
{
return _expression();
}
/** \returns an expression of the complex conjugate of \c *this.
*
* \sa adjoint() */
template<typename Derived>
EIGEN_STRONG_INLINE typename MatrixBase<Derived>::ConjugateReturnType
MatrixBase<Derived>::conjugate() const
{
return ConjugateReturnType(derived());
}
/** \returns an expression of the real part of \c *this.
*
* \sa imag() */
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::RealReturnType
MatrixBase<Derived>::real() const { return derived(); }
/** \returns an expression of the imaginary part of \c *this.
*
* \sa real() */
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ImagReturnType
MatrixBase<Derived>::imag() const { return derived(); }
/** \returns an expression of *this with the \a Scalar type casted to
* \a NewScalar.
*
* The template parameter \a NewScalar is the type we are casting the scalars to.
*
* \sa class CwiseUnaryOp
*/
template<typename Derived>
template<typename NewType>
EIGEN_STRONG_INLINE const CwiseUnaryOp<ei_scalar_cast_op<typename ei_traits<Derived>::Scalar, NewType>, Derived>
MatrixBase<Derived>::cast() const
{
return derived();
}
/** \relates MatrixBase */
template<typename Derived>
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::ScalarMultipleReturnType
MatrixBase<Derived>::operator*(const Scalar& scalar) const
{
return CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, Derived>
(derived(), ei_scalar_multiple_op<Scalar>(scalar));
}
/** \relates MatrixBase */
template<typename Derived>
EIGEN_STRONG_INLINE const CwiseUnaryOp<ei_scalar_quotient1_op<typename ei_traits<Derived>::Scalar>, Derived>
MatrixBase<Derived>::operator/(const Scalar& scalar) const
{
return CwiseUnaryOp<ei_scalar_quotient1_op<Scalar>, Derived>
(derived(), ei_scalar_quotient1_op<Scalar>(scalar));
}
template<typename Derived>
EIGEN_STRONG_INLINE Derived&
MatrixBase<Derived>::operator*=(const Scalar& other)
{
return *this = *this * other;
}
template<typename Derived>
EIGEN_STRONG_INLINE Derived&
MatrixBase<Derived>::operator/=(const Scalar& other)
{
return *this = *this / other;
}
#endif // EIGEN_CWISE_UNARY_OP_H

124
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/DiagonalCoeffs.h Normal file
View File

@ -0,0 +1,124 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_DIAGONALCOEFFS_H
#define EIGEN_DIAGONALCOEFFS_H
/** \class DiagonalCoeffs
*
* \brief Expression of the main diagonal of a matrix
*
* \param MatrixType the type of the object in which we are taking the main diagonal
*
* The matrix is not required to be square.
*
* This class represents an expression of the main diagonal of a square matrix.
* It is the return type of MatrixBase::diagonal() and most of the time this is
* the only way it is used.
*
* \sa MatrixBase::diagonal()
*/
template<typename MatrixType>
struct ei_traits<DiagonalCoeffs<MatrixType> >
{
typedef typename MatrixType::Scalar Scalar;
typedef typename ei_nested<MatrixType>::type MatrixTypeNested;
typedef typename ei_unref<MatrixTypeNested>::type _MatrixTypeNested;
enum {
RowsAtCompileTime = int(MatrixType::SizeAtCompileTime) == Dynamic ? Dynamic
: EIGEN_SIZE_MIN(MatrixType::RowsAtCompileTime,
MatrixType::ColsAtCompileTime),
ColsAtCompileTime = 1,
MaxRowsAtCompileTime = int(MatrixType::MaxSizeAtCompileTime) == Dynamic ? Dynamic
: EIGEN_SIZE_MIN(MatrixType::MaxRowsAtCompileTime,
MatrixType::MaxColsAtCompileTime),
MaxColsAtCompileTime = 1,
Flags = (unsigned int)_MatrixTypeNested::Flags & (HereditaryBits | LinearAccessBit),
CoeffReadCost = _MatrixTypeNested::CoeffReadCost
};
};
template<typename MatrixType> class DiagonalCoeffs
: public MatrixBase<DiagonalCoeffs<MatrixType> >
{
public:
EIGEN_GENERIC_PUBLIC_INTERFACE(DiagonalCoeffs)
inline DiagonalCoeffs(const MatrixType& matrix) : m_matrix(matrix) {}
EIGEN_INHERIT_ASSIGNMENT_OPERATORS(DiagonalCoeffs)
inline int rows() const { return std::min(m_matrix.rows(), m_matrix.cols()); }
inline int cols() const { return 1; }
inline Scalar& coeffRef(int row, int)
{
return m_matrix.const_cast_derived().coeffRef(row, row);
}
inline const Scalar coeff(int row, int) const
{
return m_matrix.coeff(row, row);
}
inline Scalar& coeffRef(int index)
{
return m_matrix.const_cast_derived().coeffRef(index, index);
}
inline const Scalar coeff(int index) const
{
return m_matrix.coeff(index, index);
}
protected:
const typename MatrixType::Nested m_matrix;
};
/** \returns an expression of the main diagonal of the matrix \c *this
*
* \c *this is not required to be square.
*
* Example: \include MatrixBase_diagonal.cpp
* Output: \verbinclude MatrixBase_diagonal.out
*
* \sa class DiagonalCoeffs */
template<typename Derived>
inline DiagonalCoeffs<Derived>
MatrixBase<Derived>::diagonal()
{
return DiagonalCoeffs<Derived>(derived());
}
/** This is the const version of diagonal(). */
template<typename Derived>
inline const DiagonalCoeffs<Derived>
MatrixBase<Derived>::diagonal() const
{
return DiagonalCoeffs<Derived>(derived());
}
#endif // EIGEN_DIAGONALCOEFFS_H

144
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/DiagonalMatrix.h Normal file
View File

@ -0,0 +1,144 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_DIAGONALMATRIX_H
#define EIGEN_DIAGONALMATRIX_H
/** \class DiagonalMatrix
* \nonstableyet
*
* \brief Expression of a diagonal matrix
*
* \param CoeffsVectorType the type of the vector of diagonal coefficients
*
* This class is an expression of a diagonal matrix with given vector of diagonal
* coefficients. It is the return
* type of MatrixBase::diagonal(const OtherDerived&) and most of the time this is
* the only way it is used.
*
* \sa MatrixBase::diagonal(const OtherDerived&)
*/
template<typename CoeffsVectorType>
struct ei_traits<DiagonalMatrix<CoeffsVectorType> >
{
typedef typename CoeffsVectorType::Scalar Scalar;
typedef typename ei_nested<CoeffsVectorType>::type CoeffsVectorTypeNested;
typedef typename ei_unref<CoeffsVectorTypeNested>::type _CoeffsVectorTypeNested;
enum {
RowsAtCompileTime = CoeffsVectorType::SizeAtCompileTime,
ColsAtCompileTime = CoeffsVectorType::SizeAtCompileTime,
MaxRowsAtCompileTime = CoeffsVectorType::MaxSizeAtCompileTime,
MaxColsAtCompileTime = CoeffsVectorType::MaxSizeAtCompileTime,
Flags = (_CoeffsVectorTypeNested::Flags & HereditaryBits) | Diagonal,
CoeffReadCost = _CoeffsVectorTypeNested::CoeffReadCost
};
};
template<typename CoeffsVectorType>
class DiagonalMatrix : ei_no_assignment_operator,
public MatrixBase<DiagonalMatrix<CoeffsVectorType> >
{
public:
EIGEN_GENERIC_PUBLIC_INTERFACE(DiagonalMatrix)
typedef CoeffsVectorType _CoeffsVectorType;
// needed to evaluate a DiagonalMatrix<Xpr> to a DiagonalMatrix<NestByValue<Vector> >
template<typename OtherCoeffsVectorType>
inline DiagonalMatrix(const DiagonalMatrix<OtherCoeffsVectorType>& other) : m_coeffs(other.diagonal())
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(CoeffsVectorType);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherCoeffsVectorType);
ei_assert(m_coeffs.size() > 0);
}
inline DiagonalMatrix(const CoeffsVectorType& coeffs) : m_coeffs(coeffs)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(CoeffsVectorType);
ei_assert(coeffs.size() > 0);
}
inline int rows() const { return m_coeffs.size(); }
inline int cols() const { return m_coeffs.size(); }
inline const Scalar coeff(int row, int col) const
{
return row == col ? m_coeffs.coeff(row) : static_cast<Scalar>(0);
}
inline const CoeffsVectorType& diagonal() const { return m_coeffs; }
protected:
const typename CoeffsVectorType::Nested m_coeffs;
};
/** \nonstableyet
* \returns an expression of a diagonal matrix with *this as vector of diagonal coefficients
*
* \only_for_vectors
*
* \addexample AsDiagonalExample \label How to build a diagonal matrix from a vector
*
* Example: \include MatrixBase_asDiagonal.cpp
* Output: \verbinclude MatrixBase_asDiagonal.out
*
* \sa class DiagonalMatrix, isDiagonal()
**/
template<typename Derived>
inline const DiagonalMatrix<Derived>
MatrixBase<Derived>::asDiagonal() const
{
return derived();
}
/** \nonstableyet
* \returns true if *this is approximately equal to a diagonal matrix,
* within the precision given by \a prec.
*
* Example: \include MatrixBase_isDiagonal.cpp
* Output: \verbinclude MatrixBase_isDiagonal.out
*
* \sa asDiagonal()
*/
template<typename Derived>
bool MatrixBase<Derived>::isDiagonal
(RealScalar prec) const
{
if(cols() != rows()) return false;
RealScalar maxAbsOnDiagonal = static_cast<RealScalar>(-1);
for(int j = 0; j < cols(); ++j)
{
RealScalar absOnDiagonal = ei_abs(coeff(j,j));
if(absOnDiagonal > maxAbsOnDiagonal) maxAbsOnDiagonal = absOnDiagonal;
}
for(int j = 0; j < cols(); ++j)
for(int i = 0; i < j; ++i)
{
if(!ei_isMuchSmallerThan(coeff(i, j), maxAbsOnDiagonal, prec)) return false;
if(!ei_isMuchSmallerThan(coeff(j, i), maxAbsOnDiagonal, prec)) return false;
}
return true;
}
#endif // EIGEN_DIAGONALMATRIX_H

130
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/DiagonalProduct.h Normal file
View File

@ -0,0 +1,130 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_DIAGONALPRODUCT_H
#define EIGEN_DIAGONALPRODUCT_H
/** \internal Specialization of ei_nested for DiagonalMatrix.
* Unlike ei_nested, if the argument is a DiagonalMatrix and if it must be evaluated,
* then it evaluated to a DiagonalMatrix having its own argument evaluated.
*/
template<typename T, int N> struct ei_nested_diagonal : ei_nested<T,N> {};
template<typename T, int N> struct ei_nested_diagonal<DiagonalMatrix<T>,N >
: ei_nested<DiagonalMatrix<T>, N, DiagonalMatrix<NestByValue<typename ei_plain_matrix_type<T>::type> > >
{};
// specialization of ProductReturnType
template<typename Lhs, typename Rhs>
struct ProductReturnType<Lhs,Rhs,DiagonalProduct>
{
typedef typename ei_nested_diagonal<Lhs,Rhs::ColsAtCompileTime>::type LhsNested;
typedef typename ei_nested_diagonal<Rhs,Lhs::RowsAtCompileTime>::type RhsNested;
typedef Product<LhsNested, RhsNested, DiagonalProduct> Type;
};
template<typename LhsNested, typename RhsNested>
struct ei_traits<Product<LhsNested, RhsNested, DiagonalProduct> >
{
// clean the nested types:
typedef typename ei_cleantype<LhsNested>::type _LhsNested;
typedef typename ei_cleantype<RhsNested>::type _RhsNested;
typedef typename _LhsNested::Scalar Scalar;
enum {
LhsFlags = _LhsNested::Flags,
RhsFlags = _RhsNested::Flags,
RowsAtCompileTime = _LhsNested::RowsAtCompileTime,
ColsAtCompileTime = _RhsNested::ColsAtCompileTime,
MaxRowsAtCompileTime = _LhsNested::MaxRowsAtCompileTime,
MaxColsAtCompileTime = _RhsNested::MaxColsAtCompileTime,
LhsIsDiagonal = (_LhsNested::Flags&Diagonal)==Diagonal,
RhsIsDiagonal = (_RhsNested::Flags&Diagonal)==Diagonal,
CanVectorizeRhs = (!RhsIsDiagonal) && (RhsFlags & RowMajorBit) && (RhsFlags & PacketAccessBit)
&& (ColsAtCompileTime % ei_packet_traits<Scalar>::size == 0),
CanVectorizeLhs = (!LhsIsDiagonal) && (!(LhsFlags & RowMajorBit)) && (LhsFlags & PacketAccessBit)
&& (RowsAtCompileTime % ei_packet_traits<Scalar>::size == 0),
RemovedBits = ~((RhsFlags & RowMajorBit) && (!CanVectorizeLhs) ? 0 : RowMajorBit),
Flags = ((unsigned int)(LhsFlags | RhsFlags) & HereditaryBits & RemovedBits)
| (((CanVectorizeLhs&&RhsIsDiagonal) || (CanVectorizeRhs&&LhsIsDiagonal)) ? PacketAccessBit : 0),
CoeffReadCost = NumTraits<Scalar>::MulCost + _LhsNested::CoeffReadCost + _RhsNested::CoeffReadCost
};
};
template<typename LhsNested, typename RhsNested> class Product<LhsNested, RhsNested, DiagonalProduct> : ei_no_assignment_operator,
public MatrixBase<Product<LhsNested, RhsNested, DiagonalProduct> >
{
typedef typename ei_traits<Product>::_LhsNested _LhsNested;
typedef typename ei_traits<Product>::_RhsNested _RhsNested;
enum {
RhsIsDiagonal = (_RhsNested::Flags&Diagonal)==Diagonal
};
public:
EIGEN_GENERIC_PUBLIC_INTERFACE(Product)
template<typename Lhs, typename Rhs>
inline Product(const Lhs& lhs, const Rhs& rhs)
: m_lhs(lhs), m_rhs(rhs)
{
ei_assert(lhs.cols() == rhs.rows());
}
inline int rows() const { return m_lhs.rows(); }
inline int cols() const { return m_rhs.cols(); }
const Scalar coeff(int row, int col) const
{
const int unique = RhsIsDiagonal ? col : row;
return m_lhs.coeff(row, unique) * m_rhs.coeff(unique, col);
}
template<int LoadMode>
const PacketScalar packet(int row, int col) const
{
if (RhsIsDiagonal)
{
return ei_pmul(m_lhs.template packet<LoadMode>(row, col), ei_pset1(m_rhs.coeff(col, col)));
}
else
{
return ei_pmul(ei_pset1(m_lhs.coeff(row, row)), m_rhs.template packet<LoadMode>(row, col));
}
}
protected:
const LhsNested m_lhs;
const RhsNested m_rhs;
};
#endif // EIGEN_DIAGONALPRODUCT_H

361
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Dot.h Normal file
View File

@ -0,0 +1,361 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_DOT_H
#define EIGEN_DOT_H
/***************************************************************************
* Part 1 : the logic deciding a strategy for vectorization and unrolling
***************************************************************************/
template<typename Derived1, typename Derived2>
struct ei_dot_traits
{
public:
enum {
Vectorization = (int(Derived1::Flags)&int(Derived2::Flags)&ActualPacketAccessBit)
&& (int(Derived1::Flags)&int(Derived2::Flags)&LinearAccessBit)
? LinearVectorization
: NoVectorization
};
private:
typedef typename Derived1::Scalar Scalar;
enum {
PacketSize = ei_packet_traits<Scalar>::size,
Cost = Derived1::SizeAtCompileTime * (Derived1::CoeffReadCost + Derived2::CoeffReadCost + NumTraits<Scalar>::MulCost)
+ (Derived1::SizeAtCompileTime-1) * NumTraits<Scalar>::AddCost,
UnrollingLimit = EIGEN_UNROLLING_LIMIT * (int(Vectorization) == int(NoVectorization) ? 1 : int(PacketSize))
};
public:
enum {
Unrolling = Cost <= UnrollingLimit
? CompleteUnrolling
: NoUnrolling
};
};
/***************************************************************************
* Part 2 : unrollers
***************************************************************************/
/*** no vectorization ***/
template<typename Derived1, typename Derived2, int Start, int Length>
struct ei_dot_novec_unroller
{
enum {
HalfLength = Length/2
};
typedef typename Derived1::Scalar Scalar;
inline static Scalar run(const Derived1& v1, const Derived2& v2)
{
return ei_dot_novec_unroller<Derived1, Derived2, Start, HalfLength>::run(v1, v2)
+ ei_dot_novec_unroller<Derived1, Derived2, Start+HalfLength, Length-HalfLength>::run(v1, v2);
}
};
template<typename Derived1, typename Derived2, int Start>
struct ei_dot_novec_unroller<Derived1, Derived2, Start, 1>
{
typedef typename Derived1::Scalar Scalar;
inline static Scalar run(const Derived1& v1, const Derived2& v2)
{
return v1.coeff(Start) * ei_conj(v2.coeff(Start));
}
};
/*** vectorization ***/
template<typename Derived1, typename Derived2, int Index, int Stop,
bool LastPacket = (Stop-Index == ei_packet_traits<typename Derived1::Scalar>::size)>
struct ei_dot_vec_unroller
{
typedef typename Derived1::Scalar Scalar;
typedef typename ei_packet_traits<Scalar>::type PacketScalar;
enum {
row1 = Derived1::RowsAtCompileTime == 1 ? 0 : Index,
col1 = Derived1::RowsAtCompileTime == 1 ? Index : 0,
row2 = Derived2::RowsAtCompileTime == 1 ? 0 : Index,
col2 = Derived2::RowsAtCompileTime == 1 ? Index : 0
};
inline static PacketScalar run(const Derived1& v1, const Derived2& v2)
{
return ei_pmadd(
v1.template packet<Aligned>(row1, col1),
v2.template packet<Aligned>(row2, col2),
ei_dot_vec_unroller<Derived1, Derived2, Index+ei_packet_traits<Scalar>::size, Stop>::run(v1, v2)
);
}
};
template<typename Derived1, typename Derived2, int Index, int Stop>
struct ei_dot_vec_unroller<Derived1, Derived2, Index, Stop, true>
{
enum {
row1 = Derived1::RowsAtCompileTime == 1 ? 0 : Index,
col1 = Derived1::RowsAtCompileTime == 1 ? Index : 0,
row2 = Derived2::RowsAtCompileTime == 1 ? 0 : Index,
col2 = Derived2::RowsAtCompileTime == 1 ? Index : 0,
alignment1 = (Derived1::Flags & AlignedBit) ? Aligned : Unaligned,
alignment2 = (Derived2::Flags & AlignedBit) ? Aligned : Unaligned
};
typedef typename Derived1::Scalar Scalar;
typedef typename ei_packet_traits<Scalar>::type PacketScalar;
inline static PacketScalar run(const Derived1& v1, const Derived2& v2)
{
return ei_pmul(v1.template packet<alignment1>(row1, col1), v2.template packet<alignment2>(row2, col2));
}
};
/***************************************************************************
* Part 3 : implementation of all cases
***************************************************************************/
template<typename Derived1, typename Derived2,
int Vectorization = ei_dot_traits<Derived1, Derived2>::Vectorization,
int Unrolling = ei_dot_traits<Derived1, Derived2>::Unrolling
>
struct ei_dot_impl;
template<typename Derived1, typename Derived2>
struct ei_dot_impl<Derived1, Derived2, NoVectorization, NoUnrolling>
{
typedef typename Derived1::Scalar Scalar;
static Scalar run(const Derived1& v1, const Derived2& v2)
{
ei_assert(v1.size()>0 && "you are using a non initialized vector");
Scalar res;
res = v1.coeff(0) * ei_conj(v2.coeff(0));
for(int i = 1; i < v1.size(); ++i)
res += v1.coeff(i) * ei_conj(v2.coeff(i));
return res;
}
};
template<typename Derived1, typename Derived2>
struct ei_dot_impl<Derived1, Derived2, NoVectorization, CompleteUnrolling>
: public ei_dot_novec_unroller<Derived1, Derived2, 0, Derived1::SizeAtCompileTime>
{};
template<typename Derived1, typename Derived2>
struct ei_dot_impl<Derived1, Derived2, LinearVectorization, NoUnrolling>
{
typedef typename Derived1::Scalar Scalar;
typedef typename ei_packet_traits<Scalar>::type PacketScalar;
static Scalar run(const Derived1& v1, const Derived2& v2)
{
const int size = v1.size();
const int packetSize = ei_packet_traits<Scalar>::size;
const int alignedSize = (size/packetSize)*packetSize;
enum {
alignment1 = (Derived1::Flags & AlignedBit) ? Aligned : Unaligned,
alignment2 = (Derived2::Flags & AlignedBit) ? Aligned : Unaligned
};
Scalar res;
// do the vectorizable part of the sum
if(size >= packetSize)
{
PacketScalar packet_res = ei_pmul(
v1.template packet<alignment1>(0),
v2.template packet<alignment2>(0)
);
for(int index = packetSize; index<alignedSize; index += packetSize)
{
packet_res = ei_pmadd(
v1.template packet<alignment1>(index),
v2.template packet<alignment2>(index),
packet_res
);
}
res = ei_predux(packet_res);
// now we must do the rest without vectorization.
if(alignedSize == size) return res;
}
else // too small to vectorize anything.
// since this is dynamic-size hence inefficient anyway for such small sizes, don't try to optimize.
{
res = Scalar(0);
}
// do the remainder of the vector
for(int index = alignedSize; index < size; ++index)
{
res += v1.coeff(index) * v2.coeff(index);
}
return res;
}
};
template<typename Derived1, typename Derived2>
struct ei_dot_impl<Derived1, Derived2, LinearVectorization, CompleteUnrolling>
{
typedef typename Derived1::Scalar Scalar;
typedef typename ei_packet_traits<Scalar>::type PacketScalar;
enum {
PacketSize = ei_packet_traits<Scalar>::size,
Size = Derived1::SizeAtCompileTime,
VectorizationSize = (Size / PacketSize) * PacketSize
};
static Scalar run(const Derived1& v1, const Derived2& v2)
{
Scalar res = ei_predux(ei_dot_vec_unroller<Derived1, Derived2, 0, VectorizationSize>::run(v1, v2));
if (VectorizationSize != Size)
res += ei_dot_novec_unroller<Derived1, Derived2, VectorizationSize, Size-VectorizationSize>::run(v1, v2);
return res;
}
};
/***************************************************************************
* Part 4 : implementation of MatrixBase methods
***************************************************************************/
/** \returns the dot product of *this with other.
*
* \only_for_vectors
*
* \note If the scalar type is complex numbers, then this function returns the hermitian
* (sesquilinear) dot product, linear in the first variable and conjugate-linear in the
* second variable.
*
* \sa squaredNorm(), norm()
*/
template<typename Derived>
template<typename OtherDerived>
typename ei_traits<Derived>::Scalar
MatrixBase<Derived>::dot(const MatrixBase<OtherDerived>& other) const
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived)
EIGEN_STATIC_ASSERT((ei_is_same_type<Scalar, typename OtherDerived::Scalar>::ret),
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
ei_assert(size() == other.size());
return ei_dot_impl<Derived, OtherDerived>::run(derived(), other.derived());
}
/** \returns the squared \em l2 norm of *this, i.e., for vectors, the dot product of *this with itself.
*
* \sa dot(), norm()
*/
template<typename Derived>
inline typename NumTraits<typename ei_traits<Derived>::Scalar>::Real MatrixBase<Derived>::squaredNorm() const
{
return ei_real((*this).cwise().abs2().sum());
}
/** \returns the \em l2 norm of *this, i.e., for vectors, the square root of the dot product of *this with itself.
*
* \sa dot(), squaredNorm()
*/
template<typename Derived>
inline typename NumTraits<typename ei_traits<Derived>::Scalar>::Real MatrixBase<Derived>::norm() const
{
return ei_sqrt(squaredNorm());
}
/** \returns an expression of the quotient of *this by its own norm.
*
* \only_for_vectors
*
* \sa norm(), normalize()
*/
template<typename Derived>
inline const typename MatrixBase<Derived>::PlainMatrixType
MatrixBase<Derived>::normalized() const
{
typedef typename ei_nested<Derived>::type Nested;
typedef typename ei_unref<Nested>::type _Nested;
_Nested n(derived());
return n / n.norm();
}
/** Normalizes the vector, i.e. divides it by its own norm.
*
* \only_for_vectors
*
* \sa norm(), normalized()
*/
template<typename Derived>
inline void MatrixBase<Derived>::normalize()
{
*this /= norm();
}
/** \returns true if *this is approximately orthogonal to \a other,
* within the precision given by \a prec.
*
* Example: \include MatrixBase_isOrthogonal.cpp
* Output: \verbinclude MatrixBase_isOrthogonal.out
*/
template<typename Derived>
template<typename OtherDerived>
bool MatrixBase<Derived>::isOrthogonal
(const MatrixBase<OtherDerived>& other, RealScalar prec) const
{
typename ei_nested<Derived,2>::type nested(derived());
typename ei_nested<OtherDerived,2>::type otherNested(other.derived());
return ei_abs2(nested.dot(otherNested)) <= prec * prec * nested.squaredNorm() * otherNested.squaredNorm();
}
/** \returns true if *this is approximately an unitary matrix,
* within the precision given by \a prec. In the case where the \a Scalar
* type is real numbers, a unitary matrix is an orthogonal matrix, whence the name.
*
* \note This can be used to check whether a family of vectors forms an orthonormal basis.
* Indeed, \c m.isUnitary() returns true if and only if the columns (equivalently, the rows) of m form an
* orthonormal basis.
*
* Example: \include MatrixBase_isUnitary.cpp
* Output: \verbinclude MatrixBase_isUnitary.out
*/
template<typename Derived>
bool MatrixBase<Derived>::isUnitary(RealScalar prec) const
{
typename Derived::Nested nested(derived());
for(int i = 0; i < cols(); ++i)
{
if(!ei_isApprox(nested.col(i).squaredNorm(), static_cast<Scalar>(1), prec))
return false;
for(int j = 0; j < i; ++j)
if(!ei_isMuchSmallerThan(nested.col(i).dot(nested.col(j)), static_cast<Scalar>(1), prec))
return false;
}
return true;
}
#endif // EIGEN_DOT_H

149
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Flagged.h Normal file
View File

@ -0,0 +1,149 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_FLAGGED_H
#define EIGEN_FLAGGED_H
/** \class Flagged
*
* \brief Expression with modified flags
*
* \param ExpressionType the type of the object of which we are modifying the flags
* \param Added the flags added to the expression
* \param Removed the flags removed from the expression (has priority over Added).
*
* This class represents an expression whose flags have been modified.
* It is the return type of MatrixBase::flagged()
* and most of the time this is the only way it is used.
*
* \sa MatrixBase::flagged()
*/
template<typename ExpressionType, unsigned int Added, unsigned int Removed>
struct ei_traits<Flagged<ExpressionType, Added, Removed> > : ei_traits<ExpressionType>
{
enum { Flags = (ExpressionType::Flags | Added) & ~Removed };
};
template<typename ExpressionType, unsigned int Added, unsigned int Removed> class Flagged
: public MatrixBase<Flagged<ExpressionType, Added, Removed> >
{
public:
EIGEN_GENERIC_PUBLIC_INTERFACE(Flagged)
typedef typename ei_meta_if<ei_must_nest_by_value<ExpressionType>::ret,
ExpressionType, const ExpressionType&>::ret ExpressionTypeNested;
typedef typename ExpressionType::InnerIterator InnerIterator;
inline Flagged(const ExpressionType& matrix) : m_matrix(matrix) {}
inline int rows() const { return m_matrix.rows(); }
inline int cols() const { return m_matrix.cols(); }
inline int stride() const { return m_matrix.stride(); }
inline const Scalar coeff(int row, int col) const
{
return m_matrix.coeff(row, col);
}
inline Scalar& coeffRef(int row, int col)
{
return m_matrix.const_cast_derived().coeffRef(row, col);
}
inline const Scalar coeff(int index) const
{
return m_matrix.coeff(index);
}
inline Scalar& coeffRef(int index)
{
return m_matrix.const_cast_derived().coeffRef(index);
}
template<int LoadMode>
inline const PacketScalar packet(int row, int col) const
{
return m_matrix.template packet<LoadMode>(row, col);
}
template<int LoadMode>
inline void writePacket(int row, int col, const PacketScalar& x)
{
m_matrix.const_cast_derived().template writePacket<LoadMode>(row, col, x);
}
template<int LoadMode>
inline const PacketScalar packet(int index) const
{
return m_matrix.template packet<LoadMode>(index);
}
template<int LoadMode>
inline void writePacket(int index, const PacketScalar& x)
{
m_matrix.const_cast_derived().template writePacket<LoadMode>(index, x);
}
const ExpressionType& _expression() const { return m_matrix; }
protected:
ExpressionTypeNested m_matrix;
private:
Flagged& operator=(const Flagged&);
};
/** \returns an expression of *this with added flags
*
* \addexample MarkExample \label How to mark a triangular matrix as triangular
*
* Example: \include MatrixBase_marked.cpp
* Output: \verbinclude MatrixBase_marked.out
*
* \sa class Flagged, extract(), part()
*/
template<typename Derived>
template<unsigned int Added>
inline const Flagged<Derived, Added, 0>
MatrixBase<Derived>::marked() const
{
return derived();
}
/** \returns an expression of *this with the following flags removed:
* EvalBeforeNestingBit and EvalBeforeAssigningBit.
*
* Example: \include MatrixBase_lazy.cpp
* Output: \verbinclude MatrixBase_lazy.out
*
* \sa class Flagged, marked()
*/
template<typename Derived>
inline const Flagged<Derived, 0, EvalBeforeNestingBit | EvalBeforeAssigningBit>
MatrixBase<Derived>::lazy() const
{
return derived();
}
#endif // EIGEN_FLAGGED_H

378
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Functors.h Normal file
View File

@ -0,0 +1,378 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_FUNCTORS_H
#define EIGEN_FUNCTORS_H
// associative functors:
/** \internal
* \brief Template functor to compute the sum of two scalars
*
* \sa class CwiseBinaryOp, MatrixBase::operator+, class PartialRedux, MatrixBase::sum()
*/
template<typename Scalar> struct ei_scalar_sum_op EIGEN_EMPTY_STRUCT {
EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return a + b; }
template<typename PacketScalar>
EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a, const PacketScalar& b) const
{ return ei_padd(a,b); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_sum_op<Scalar> > {
enum {
Cost = NumTraits<Scalar>::AddCost,
PacketAccess = ei_packet_traits<Scalar>::size>1
};
};
/** \internal
* \brief Template functor to compute the product of two scalars
*
* \sa class CwiseBinaryOp, Cwise::operator*(), class PartialRedux, MatrixBase::redux()
*/
template<typename Scalar> struct ei_scalar_product_op EIGEN_EMPTY_STRUCT {
EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return a * b; }
template<typename PacketScalar>
EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a, const PacketScalar& b) const
{ return ei_pmul(a,b); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_product_op<Scalar> > {
enum {
Cost = NumTraits<Scalar>::MulCost,
PacketAccess = ei_packet_traits<Scalar>::size>1
};
};
/** \internal
* \brief Template functor to compute the min of two scalars
*
* \sa class CwiseBinaryOp, MatrixBase::cwiseMin, class PartialRedux, MatrixBase::minCoeff()
*/
template<typename Scalar> struct ei_scalar_min_op EIGEN_EMPTY_STRUCT {
EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return std::min(a, b); }
template<typename PacketScalar>
EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a, const PacketScalar& b) const
{ return ei_pmin(a,b); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_min_op<Scalar> > {
enum {
Cost = NumTraits<Scalar>::AddCost,
PacketAccess = ei_packet_traits<Scalar>::size>1
};
};
/** \internal
* \brief Template functor to compute the max of two scalars
*
* \sa class CwiseBinaryOp, MatrixBase::cwiseMax, class PartialRedux, MatrixBase::maxCoeff()
*/
template<typename Scalar> struct ei_scalar_max_op EIGEN_EMPTY_STRUCT {
EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return std::max(a, b); }
template<typename PacketScalar>
EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a, const PacketScalar& b) const
{ return ei_pmax(a,b); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_max_op<Scalar> > {
enum {
Cost = NumTraits<Scalar>::AddCost,
PacketAccess = ei_packet_traits<Scalar>::size>1
};
};
// other binary functors:
/** \internal
* \brief Template functor to compute the difference of two scalars
*
* \sa class CwiseBinaryOp, MatrixBase::operator-
*/
template<typename Scalar> struct ei_scalar_difference_op EIGEN_EMPTY_STRUCT {
EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return a - b; }
template<typename PacketScalar>
EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a, const PacketScalar& b) const
{ return ei_psub(a,b); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_difference_op<Scalar> > {
enum {
Cost = NumTraits<Scalar>::AddCost,
PacketAccess = ei_packet_traits<Scalar>::size>1
};
};
/** \internal
* \brief Template functor to compute the quotient of two scalars
*
* \sa class CwiseBinaryOp, Cwise::operator/()
*/
template<typename Scalar> struct ei_scalar_quotient_op EIGEN_EMPTY_STRUCT {
EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a, const Scalar& b) const { return a / b; }
template<typename PacketScalar>
EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a, const PacketScalar& b) const
{ return ei_pdiv(a,b); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_quotient_op<Scalar> > {
enum {
Cost = 2 * NumTraits<Scalar>::MulCost,
PacketAccess = ei_packet_traits<Scalar>::size>1
#if (defined EIGEN_VECTORIZE_SSE)
&& NumTraits<Scalar>::HasFloatingPoint
#endif
};
};
// unary functors:
/** \internal
* \brief Template functor to compute the opposite of a scalar
*
* \sa class CwiseUnaryOp, MatrixBase::operator-
*/
template<typename Scalar> struct ei_scalar_opposite_op EIGEN_EMPTY_STRUCT {
EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { return -a; }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_opposite_op<Scalar> >
{ enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = false }; };
/** \internal
* \brief Template functor to compute the absolute value of a scalar
*
* \sa class CwiseUnaryOp, Cwise::abs
*/
template<typename Scalar> struct ei_scalar_abs_op EIGEN_EMPTY_STRUCT {
typedef typename NumTraits<Scalar>::Real result_type;
EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a) const { return ei_abs(a); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_abs_op<Scalar> >
{
enum {
Cost = NumTraits<Scalar>::AddCost,
PacketAccess = false // this could actually be vectorized with SSSE3.
};
};
/** \internal
* \brief Template functor to compute the squared absolute value of a scalar
*
* \sa class CwiseUnaryOp, Cwise::abs2
*/
template<typename Scalar> struct ei_scalar_abs2_op EIGEN_EMPTY_STRUCT {
typedef typename NumTraits<Scalar>::Real result_type;
EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a) const { return ei_abs2(a); }
template<typename PacketScalar>
EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a) const
{ return ei_pmul(a,a); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_abs2_op<Scalar> >
{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = int(ei_packet_traits<Scalar>::size)>1 }; };
/** \internal
* \brief Template functor to compute the conjugate of a complex value
*
* \sa class CwiseUnaryOp, MatrixBase::conjugate()
*/
template<typename Scalar> struct ei_scalar_conjugate_op EIGEN_EMPTY_STRUCT {
EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { return ei_conj(a); }
template<typename PacketScalar>
EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a) const { return a; }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_conjugate_op<Scalar> >
{
enum {
Cost = NumTraits<Scalar>::IsComplex ? NumTraits<Scalar>::AddCost : 0,
PacketAccess = int(ei_packet_traits<Scalar>::size)>1
};
};
/** \internal
* \brief Template functor to cast a scalar to another type
*
* \sa class CwiseUnaryOp, MatrixBase::cast()
*/
template<typename Scalar, typename NewType>
struct ei_scalar_cast_op EIGEN_EMPTY_STRUCT {
typedef NewType result_type;
EIGEN_STRONG_INLINE const NewType operator() (const Scalar& a) const { return static_cast<NewType>(a); }
};
template<typename Scalar, typename NewType>
struct ei_functor_traits<ei_scalar_cast_op<Scalar,NewType> >
{ enum { Cost = ei_is_same_type<Scalar, NewType>::ret ? 0 : NumTraits<NewType>::AddCost, PacketAccess = false }; };
/** \internal
* \brief Template functor to extract the real part of a complex
*
* \sa class CwiseUnaryOp, MatrixBase::real()
*/
template<typename Scalar>
struct ei_scalar_real_op EIGEN_EMPTY_STRUCT {
typedef typename NumTraits<Scalar>::Real result_type;
EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const { return ei_real(a); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_real_op<Scalar> >
{ enum { Cost = 0, PacketAccess = false }; };
/** \internal
* \brief Template functor to extract the imaginary part of a complex
*
* \sa class CwiseUnaryOp, MatrixBase::imag()
*/
template<typename Scalar>
struct ei_scalar_imag_op EIGEN_EMPTY_STRUCT {
typedef typename NumTraits<Scalar>::Real result_type;
EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const { return ei_imag(a); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_imag_op<Scalar> >
{ enum { Cost = 0, PacketAccess = false }; };
/** \internal
* \brief Template functor to multiply a scalar by a fixed other one
*
* \sa class CwiseUnaryOp, MatrixBase::operator*, MatrixBase::operator/
*/
/* NOTE why doing the ei_pset1() in packetOp *is* an optimization ?
* indeed it seems better to declare m_other as a PacketScalar and do the ei_pset1() once
* in the constructor. However, in practice:
* - GCC does not like m_other as a PacketScalar and generate a load every time it needs it
* - one the other hand GCC is able to moves the ei_pset1() away the loop :)
* - simpler code ;)
* (ICC and gcc 4.4 seems to perform well in both cases, the issue is visible with y = a*x + b*y)
*/
template<typename Scalar>
struct ei_scalar_multiple_op {
typedef typename ei_packet_traits<Scalar>::type PacketScalar;
// FIXME default copy constructors seems bugged with std::complex<>
EIGEN_STRONG_INLINE ei_scalar_multiple_op(const ei_scalar_multiple_op& other) : m_other(other.m_other) { }
EIGEN_STRONG_INLINE ei_scalar_multiple_op(const Scalar& other) : m_other(other) { }
EIGEN_STRONG_INLINE Scalar operator() (const Scalar& a) const { return a * m_other; }
EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a) const
{ return ei_pmul(a, ei_pset1(m_other)); }
const Scalar m_other;
private:
ei_scalar_multiple_op& operator=(const ei_scalar_multiple_op&);
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_multiple_op<Scalar> >
{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = ei_packet_traits<Scalar>::size>1 }; };
template<typename Scalar, bool HasFloatingPoint>
struct ei_scalar_quotient1_impl {
typedef typename ei_packet_traits<Scalar>::type PacketScalar;
// FIXME default copy constructors seems bugged with std::complex<>
EIGEN_STRONG_INLINE ei_scalar_quotient1_impl(const ei_scalar_quotient1_impl& other) : m_other(other.m_other) { }
EIGEN_STRONG_INLINE ei_scalar_quotient1_impl(const Scalar& other) : m_other(static_cast<Scalar>(1) / other) {}
EIGEN_STRONG_INLINE Scalar operator() (const Scalar& a) const { return a * m_other; }
EIGEN_STRONG_INLINE const PacketScalar packetOp(const PacketScalar& a) const
{ return ei_pmul(a, ei_pset1(m_other)); }
const Scalar m_other;
private:
ei_scalar_quotient1_impl& operator=(const ei_scalar_quotient1_impl&);
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_quotient1_impl<Scalar,true> >
{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = ei_packet_traits<Scalar>::size>1 }; };
template<typename Scalar>
struct ei_scalar_quotient1_impl<Scalar,false> {
// FIXME default copy constructors seems bugged with std::complex<>
EIGEN_STRONG_INLINE ei_scalar_quotient1_impl(const ei_scalar_quotient1_impl& other) : m_other(other.m_other) { }
EIGEN_STRONG_INLINE ei_scalar_quotient1_impl(const Scalar& other) : m_other(other) {}
EIGEN_STRONG_INLINE Scalar operator() (const Scalar& a) const { return a / m_other; }
const Scalar m_other;
private:
ei_scalar_quotient1_impl& operator=(const ei_scalar_quotient1_impl&);
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_quotient1_impl<Scalar,false> >
{ enum { Cost = 2 * NumTraits<Scalar>::MulCost, PacketAccess = false }; };
/** \internal
* \brief Template functor to divide a scalar by a fixed other one
*
* This functor is used to implement the quotient of a matrix by
* a scalar where the scalar type is not necessarily a floating point type.
*
* \sa class CwiseUnaryOp, MatrixBase::operator/
*/
template<typename Scalar>
struct ei_scalar_quotient1_op : ei_scalar_quotient1_impl<Scalar, NumTraits<Scalar>::HasFloatingPoint > {
EIGEN_STRONG_INLINE ei_scalar_quotient1_op(const Scalar& other)
: ei_scalar_quotient1_impl<Scalar, NumTraits<Scalar>::HasFloatingPoint >(other) {}
private:
ei_scalar_quotient1_op& operator=(const ei_scalar_quotient1_op&);
};
// nullary functors
template<typename Scalar>
struct ei_scalar_constant_op {
typedef typename ei_packet_traits<Scalar>::type PacketScalar;
EIGEN_STRONG_INLINE ei_scalar_constant_op(const ei_scalar_constant_op& other) : m_other(other.m_other) { }
EIGEN_STRONG_INLINE ei_scalar_constant_op(const Scalar& other) : m_other(other) { }
EIGEN_STRONG_INLINE const Scalar operator() (int, int = 0) const { return m_other; }
EIGEN_STRONG_INLINE const PacketScalar packetOp() const { return ei_pset1(m_other); }
const Scalar m_other;
private:
ei_scalar_constant_op& operator=(const ei_scalar_constant_op&);
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_constant_op<Scalar> >
{ enum { Cost = 1, PacketAccess = ei_packet_traits<Scalar>::size>1, IsRepeatable = true }; };
template<typename Scalar> struct ei_scalar_identity_op EIGEN_EMPTY_STRUCT {
EIGEN_STRONG_INLINE ei_scalar_identity_op(void) {}
EIGEN_STRONG_INLINE const Scalar operator() (int row, int col) const { return row==col ? Scalar(1) : Scalar(0); }
};
template<typename Scalar>
struct ei_functor_traits<ei_scalar_identity_op<Scalar> >
{ enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = false, IsRepeatable = true }; };
// allow to add new functors and specializations of ei_functor_traits from outside Eigen.
// this macro is really needed because ei_functor_traits must be specialized after it is declared but before it is used...
#ifdef EIGEN_FUNCTORS_PLUGIN
#include EIGEN_FUNCTORS_PLUGIN
#endif
// all functors allow linear access, except ei_scalar_identity_op. So we fix here a quick meta
// to indicate whether a functor allows linear access, just always answering 'yes' except for
// ei_scalar_identity_op.
template<typename Functor> struct ei_functor_has_linear_access { enum { ret = 1 }; };
template<typename Scalar> struct ei_functor_has_linear_access<ei_scalar_identity_op<Scalar> > { enum { ret = 0 }; };
// in CwiseBinaryOp, we require the Lhs and Rhs to have the same scalar type, except for multiplication
// where we only require them to have the same _real_ scalar type so one may multiply, say, float by complex<float>.
template<typename Functor> struct ei_functor_allows_mixing_real_and_complex { enum { ret = 0 }; };
template<typename Scalar> struct ei_functor_allows_mixing_real_and_complex<ei_scalar_product_op<Scalar> > { enum { ret = 1 }; };
#endif // EIGEN_FUNCTORS_H

234
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Fuzzy.h Normal file
View File

@ -0,0 +1,234 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_FUZZY_H
#define EIGEN_FUZZY_H
#ifndef EIGEN_LEGACY_COMPARES
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \note The fuzzy compares are done multiplicatively. Two vectors \f$ v \f$ and \f$ w \f$
* are considered to be approximately equal within precision \f$ p \f$ if
* \f[ \Vert v - w \Vert \leqslant p\,\min(\Vert v\Vert, \Vert w\Vert). \f]
* For matrices, the comparison is done using the Hilbert-Schmidt norm (aka Frobenius norm
* L2 norm).
*
* \note Because of the multiplicativeness of this comparison, one can't use this function
* to check whether \c *this is approximately equal to the zero matrix or vector.
* Indeed, \c isApprox(zero) returns false unless \c *this itself is exactly the zero matrix
* or vector. If you want to test whether \c *this is zero, use ei_isMuchSmallerThan(const
* RealScalar&, RealScalar) instead.
*
* \sa ei_isMuchSmallerThan(const RealScalar&, RealScalar) const
*/
template<typename Derived>
template<typename OtherDerived>
bool MatrixBase<Derived>::isApprox(
const MatrixBase<OtherDerived>& other,
typename NumTraits<Scalar>::Real prec
) const
{
const typename ei_nested<Derived,2>::type nested(derived());
const typename ei_nested<OtherDerived,2>::type otherNested(other.derived());
return (nested - otherNested).cwise().abs2().sum() <= prec * prec * std::min(nested.cwise().abs2().sum(), otherNested.cwise().abs2().sum());
}
/** \returns \c true if the norm of \c *this is much smaller than \a other,
* within the precision determined by \a prec.
*
* \note The fuzzy compares are done multiplicatively. A vector \f$ v \f$ is
* considered to be much smaller than \f$ x \f$ within precision \f$ p \f$ if
* \f[ \Vert v \Vert \leqslant p\,\vert x\vert. \f]
*
* For matrices, the comparison is done using the Hilbert-Schmidt norm. For this reason,
* the value of the reference scalar \a other should come from the Hilbert-Schmidt norm
* of a reference matrix of same dimensions.
*
* \sa isApprox(), isMuchSmallerThan(const MatrixBase<OtherDerived>&, RealScalar) const
*/
template<typename Derived>
bool MatrixBase<Derived>::isMuchSmallerThan(
const typename NumTraits<Scalar>::Real& other,
typename NumTraits<Scalar>::Real prec
) const
{
return cwise().abs2().sum() <= prec * prec * other * other;
}
/** \returns \c true if the norm of \c *this is much smaller than the norm of \a other,
* within the precision determined by \a prec.
*
* \note The fuzzy compares are done multiplicatively. A vector \f$ v \f$ is
* considered to be much smaller than a vector \f$ w \f$ within precision \f$ p \f$ if
* \f[ \Vert v \Vert \leqslant p\,\Vert w\Vert. \f]
* For matrices, the comparison is done using the Hilbert-Schmidt norm.
*
* \sa isApprox(), isMuchSmallerThan(const RealScalar&, RealScalar) const
*/
template<typename Derived>
template<typename OtherDerived>
bool MatrixBase<Derived>::isMuchSmallerThan(
const MatrixBase<OtherDerived>& other,
typename NumTraits<Scalar>::Real prec
) const
{
return this->cwise().abs2().sum() <= prec * prec * other.cwise().abs2().sum();
}
#else
template<typename Derived, typename OtherDerived=Derived, bool IsVector=Derived::IsVectorAtCompileTime>
struct ei_fuzzy_selector;
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \note The fuzzy compares are done multiplicatively. Two vectors \f$ v \f$ and \f$ w \f$
* are considered to be approximately equal within precision \f$ p \f$ if
* \f[ \Vert v - w \Vert \leqslant p\,\min(\Vert v\Vert, \Vert w\Vert). \f]
* For matrices, the comparison is done on all columns.
*
* \note Because of the multiplicativeness of this comparison, one can't use this function
* to check whether \c *this is approximately equal to the zero matrix or vector.
* Indeed, \c isApprox(zero) returns false unless \c *this itself is exactly the zero matrix
* or vector. If you want to test whether \c *this is zero, use ei_isMuchSmallerThan(const
* RealScalar&, RealScalar) instead.
*
* \sa ei_isMuchSmallerThan(const RealScalar&, RealScalar) const
*/
template<typename Derived>
template<typename OtherDerived>
bool MatrixBase<Derived>::isApprox(
const MatrixBase<OtherDerived>& other,
typename NumTraits<Scalar>::Real prec
) const
{
return ei_fuzzy_selector<Derived,OtherDerived>::isApprox(derived(), other.derived(), prec);
}
/** \returns \c true if the norm of \c *this is much smaller than \a other,
* within the precision determined by \a prec.
*
* \note The fuzzy compares are done multiplicatively. A vector \f$ v \f$ is
* considered to be much smaller than \f$ x \f$ within precision \f$ p \f$ if
* \f[ \Vert v \Vert \leqslant p\,\vert x\vert. \f]
* For matrices, the comparison is done on all columns.
*
* \sa isApprox(), isMuchSmallerThan(const MatrixBase<OtherDerived>&, RealScalar) const
*/
template<typename Derived>
bool MatrixBase<Derived>::isMuchSmallerThan(
const typename NumTraits<Scalar>::Real& other,
typename NumTraits<Scalar>::Real prec
) const
{
return ei_fuzzy_selector<Derived>::isMuchSmallerThan(derived(), other, prec);
}
/** \returns \c true if the norm of \c *this is much smaller than the norm of \a other,
* within the precision determined by \a prec.
*
* \note The fuzzy compares are done multiplicatively. A vector \f$ v \f$ is
* considered to be much smaller than a vector \f$ w \f$ within precision \f$ p \f$ if
* \f[ \Vert v \Vert \leqslant p\,\Vert w\Vert. \f]
* For matrices, the comparison is done on all columns.
*
* \sa isApprox(), isMuchSmallerThan(const RealScalar&, RealScalar) const
*/
template<typename Derived>
template<typename OtherDerived>
bool MatrixBase<Derived>::isMuchSmallerThan(
const MatrixBase<OtherDerived>& other,
typename NumTraits<Scalar>::Real prec
) const
{
return ei_fuzzy_selector<Derived,OtherDerived>::isMuchSmallerThan(derived(), other.derived(), prec);
}
template<typename Derived, typename OtherDerived>
struct ei_fuzzy_selector<Derived,OtherDerived,true>
{
typedef typename Derived::RealScalar RealScalar;
static bool isApprox(const Derived& self, const OtherDerived& other, RealScalar prec)
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived)
ei_assert(self.size() == other.size());
return((self - other).squaredNorm() <= std::min(self.squaredNorm(), other.squaredNorm()) * prec * prec);
}
static bool isMuchSmallerThan(const Derived& self, const RealScalar& other, RealScalar prec)
{
return(self.squaredNorm() <= ei_abs2(other * prec));
}
static bool isMuchSmallerThan(const Derived& self, const OtherDerived& other, RealScalar prec)
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Derived,OtherDerived)
ei_assert(self.size() == other.size());
return(self.squaredNorm() <= other.squaredNorm() * prec * prec);
}
};
template<typename Derived, typename OtherDerived>
struct ei_fuzzy_selector<Derived,OtherDerived,false>
{
typedef typename Derived::RealScalar RealScalar;
static bool isApprox(const Derived& self, const OtherDerived& other, RealScalar prec)
{
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived,OtherDerived)
ei_assert(self.rows() == other.rows() && self.cols() == other.cols());
typename Derived::Nested nested(self);
typename OtherDerived::Nested otherNested(other);
for(int i = 0; i < self.cols(); ++i)
if((nested.col(i) - otherNested.col(i)).squaredNorm()
> std::min(nested.col(i).squaredNorm(), otherNested.col(i).squaredNorm()) * prec * prec)
return false;
return true;
}
static bool isMuchSmallerThan(const Derived& self, const RealScalar& other, RealScalar prec)
{
typename Derived::Nested nested(self);
for(int i = 0; i < self.cols(); ++i)
if(nested.col(i).squaredNorm() > ei_abs2(other * prec))
return false;
return true;
}
static bool isMuchSmallerThan(const Derived& self, const OtherDerived& other, RealScalar prec)
{
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived,OtherDerived)
ei_assert(self.rows() == other.rows() && self.cols() == other.cols());
typename Derived::Nested nested(self);
typename OtherDerived::Nested otherNested(other);
for(int i = 0; i < self.cols(); ++i)
if(nested.col(i).squaredNorm() > otherNested.col(i).squaredNorm() * prec * prec)
return false;
return true;
}
};
#endif
#endif // EIGEN_FUZZY_H

150
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/GenericPacketMath.h Normal file
View File

@ -0,0 +1,150 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_GENERIC_PACKET_MATH_H
#define EIGEN_GENERIC_PACKET_MATH_H
/** \internal
* \file GenericPacketMath.h
*
* Default implementation for types not supported by the vectorization.
* In practice these functions are provided to make easier the writing
* of generic vectorized code.
*/
/** \internal \returns a + b (coeff-wise) */
template<typename Packet> inline Packet
ei_padd(const Packet& a,
const Packet& b) { return a+b; }
/** \internal \returns a - b (coeff-wise) */
template<typename Packet> inline Packet
ei_psub(const Packet& a,
const Packet& b) { return a-b; }
/** \internal \returns a * b (coeff-wise) */
template<typename Packet> inline Packet
ei_pmul(const Packet& a,
const Packet& b) { return a*b; }
/** \internal \returns a / b (coeff-wise) */
template<typename Packet> inline Packet
ei_pdiv(const Packet& a,
const Packet& b) { return a/b; }
/** \internal \returns the min of \a a and \a b (coeff-wise) */
template<typename Packet> inline Packet
ei_pmin(const Packet& a,
const Packet& b) { return std::min(a, b); }
/** \internal \returns the max of \a a and \a b (coeff-wise) */
template<typename Packet> inline Packet
ei_pmax(const Packet& a,
const Packet& b) { return std::max(a, b); }
/** \internal \returns a packet version of \a *from, from must be 16 bytes aligned */
template<typename Scalar> inline typename ei_packet_traits<Scalar>::type
ei_pload(const Scalar* from) { return *from; }
/** \internal \returns a packet version of \a *from, (un-aligned load) */
template<typename Scalar> inline typename ei_packet_traits<Scalar>::type
ei_ploadu(const Scalar* from) { return *from; }
/** \internal \returns a packet with constant coefficients \a a, e.g.: (a,a,a,a) */
template<typename Scalar> inline typename ei_packet_traits<Scalar>::type
ei_pset1(const Scalar& a) { return a; }
/** \internal copy the packet \a from to \a *to, \a to must be 16 bytes aligned */
template<typename Scalar, typename Packet> inline void ei_pstore(Scalar* to, const Packet& from)
{ (*to) = from; }
/** \internal copy the packet \a from to \a *to, (un-aligned store) */
template<typename Scalar, typename Packet> inline void ei_pstoreu(Scalar* to, const Packet& from)
{ (*to) = from; }
/** \internal \returns the first element of a packet */
template<typename Packet> inline typename ei_unpacket_traits<Packet>::type ei_pfirst(const Packet& a)
{ return a; }
/** \internal \returns a packet where the element i contains the sum of the packet of \a vec[i] */
template<typename Packet> inline Packet
ei_preduxp(const Packet* vecs) { return vecs[0]; }
/** \internal \returns the sum of the elements of \a a*/
template<typename Packet> inline typename ei_unpacket_traits<Packet>::type ei_predux(const Packet& a)
{ return a; }
/***************************************************************************
* The following functions might not have to be overwritten for vectorized types
***************************************************************************/
/** \internal \returns a * b + c (coeff-wise) */
template<typename Packet> inline Packet
ei_pmadd(const Packet& a,
const Packet& b,
const Packet& c)
{ return ei_padd(ei_pmul(a, b),c); }
/** \internal \returns a packet version of \a *from.
* \If LoadMode equals Aligned, \a from must be 16 bytes aligned */
template<typename Scalar, int LoadMode>
inline typename ei_packet_traits<Scalar>::type ei_ploadt(const Scalar* from)
{
if(LoadMode == Aligned)
return ei_pload(from);
else
return ei_ploadu(from);
}
/** \internal copy the packet \a from to \a *to.
* If StoreMode equals Aligned, \a to must be 16 bytes aligned */
template<typename Scalar, typename Packet, int LoadMode>
inline void ei_pstoret(Scalar* to, const Packet& from)
{
if(LoadMode == Aligned)
ei_pstore(to, from);
else
ei_pstoreu(to, from);
}
/** \internal default implementation of ei_palign() allowing partial specialization */
template<int Offset,typename PacketType>
struct ei_palign_impl
{
// by default data are aligned, so there is nothing to be done :)
inline static void run(PacketType&, const PacketType&) {}
};
/** \internal update \a first using the concatenation of the \a Offset last elements
* of \a first and packet_size minus \a Offset first elements of \a second */
template<int Offset,typename PacketType>
inline void ei_palign(PacketType& first, const PacketType& second)
{
ei_palign_impl<Offset,PacketType>::run(first,second);
}
#endif // EIGEN_GENERIC_PACKET_MATH_H

184
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/IO.h Normal file
View File

@ -0,0 +1,184 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_IO_H
#define EIGEN_IO_H
enum { Raw, AlignCols };
/** \class IOFormat
*
* \brief Stores a set of parameters controlling the way matrices are printed
*
* List of available parameters:
* - \b precision number of digits for floating point values
* - \b flags can be either Raw (default) or AlignCols which aligns all the columns
* - \b coeffSeparator string printed between two coefficients of the same row
* - \b rowSeparator string printed between two rows
* - \b rowPrefix string printed at the beginning of each row
* - \b rowSuffix string printed at the end of each row
* - \b matPrefix string printed at the beginning of the matrix
* - \b matSuffix string printed at the end of the matrix
*
* Example: \include IOFormat.cpp
* Output: \verbinclude IOFormat.out
*
* \sa MatrixBase::format(), class WithFormat
*/
struct IOFormat
{
/** Default contructor, see class IOFormat for the meaning of the parameters */
IOFormat(int _precision=4, int _flags=Raw,
const std::string& _coeffSeparator = " ",
const std::string& _rowSeparator = "\n", const std::string& _rowPrefix="", const std::string& _rowSuffix="",
const std::string& _matPrefix="", const std::string& _matSuffix="")
: matPrefix(_matPrefix), matSuffix(_matSuffix), rowPrefix(_rowPrefix), rowSuffix(_rowSuffix), rowSeparator(_rowSeparator),
coeffSeparator(_coeffSeparator), precision(_precision), flags(_flags)
{
rowSpacer = "";
int i = int(matSuffix.length())-1;
while (i>=0 && matSuffix[i]!='\n')
{
rowSpacer += ' ';
i--;
}
}
std::string matPrefix, matSuffix;
std::string rowPrefix, rowSuffix, rowSeparator, rowSpacer;
std::string coeffSeparator;
int precision;
int flags;
};
/** \class WithFormat
*
* \brief Pseudo expression providing matrix output with given format
*
* \param ExpressionType the type of the object on which IO stream operations are performed
*
* This class represents an expression with stream operators controlled by a given IOFormat.
* It is the return type of MatrixBase::format()
* and most of the time this is the only way it is used.
*
* See class IOFormat for some examples.
*
* \sa MatrixBase::format(), class IOFormat
*/
template<typename ExpressionType>
class WithFormat
{
public:
WithFormat(const ExpressionType& matrix, const IOFormat& format)
: m_matrix(matrix), m_format(format)
{}
friend std::ostream & operator << (std::ostream & s, const WithFormat& wf)
{
return ei_print_matrix(s, wf.m_matrix.eval(), wf.m_format);
}
protected:
const typename ExpressionType::Nested m_matrix;
IOFormat m_format;
};
/** \returns a WithFormat proxy object allowing to print a matrix the with given
* format \a fmt.
*
* See class IOFormat for some examples.
*
* \sa class IOFormat, class WithFormat
*/
template<typename Derived>
inline const WithFormat<Derived>
MatrixBase<Derived>::format(const IOFormat& fmt) const
{
return WithFormat<Derived>(derived(), fmt);
}
/** \internal
* print the matrix \a _m to the output stream \a s using the output format \a fmt */
template<typename Derived>
std::ostream & ei_print_matrix(std::ostream & s, const Derived& _m, const IOFormat& fmt)
{
const typename Derived::Nested m = _m;
int width = 0;
if (fmt.flags & AlignCols)
{
// compute the largest width
for(int j = 1; j < m.cols(); ++j)
for(int i = 0; i < m.rows(); ++i)
{
std::stringstream sstr;
sstr.precision(fmt.precision);
sstr << m.coeff(i,j);
width = std::max<int>(width, int(sstr.str().length()));
}
}
s.precision(fmt.precision);
s << fmt.matPrefix;
for(int i = 0; i < m.rows(); ++i)
{
if (i)
s << fmt.rowSpacer;
s << fmt.rowPrefix;
if(width) s.width(width);
s << m.coeff(i, 0);
for(int j = 1; j < m.cols(); ++j)
{
s << fmt.coeffSeparator;
if (width) s.width(width);
s << m.coeff(i, j);
}
s << fmt.rowSuffix;
if( i < m.rows() - 1)
s << fmt.rowSeparator;
}
s << fmt.matSuffix;
return s;
}
/** \relates MatrixBase
*
* Outputs the matrix, to the given stream.
*
* If you wish to print the matrix with a format different than the default, use MatrixBase::format().
*
* It is also possible to change the default format by defining EIGEN_DEFAULT_IO_FORMAT before including Eigen headers.
* If not defined, this will automatically be defined to Eigen::IOFormat(), that is the Eigen::IOFormat with default parameters.
*
* \sa MatrixBase::format()
*/
template<typename Derived>
std::ostream & operator <<
(std::ostream & s,
const MatrixBase<Derived> & m)
{
return ei_print_matrix(s, m.eval(), EIGEN_DEFAULT_IO_FORMAT);
}
#endif // EIGEN_IO_H

111
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Map.h Normal file
View File

@ -0,0 +1,111 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_MAP_H
#define EIGEN_MAP_H
/** \class Map
*
* \brief A matrix or vector expression mapping an existing array of data.
*
* \param MatrixType the equivalent matrix type of the mapped data
* \param _PacketAccess allows to enforce aligned loads and stores if set to ForceAligned.
* The default is AsRequested. This parameter is internaly used by Eigen
* in expressions such as \code Map<...>(...) += other; \endcode and most
* of the time this is the only way it is used.
*
* This class represents a matrix or vector expression mapping an existing array of data.
* It can be used to let Eigen interface without any overhead with non-Eigen data structures,
* such as plain C arrays or structures from other libraries.
*
* This class is the return type of Matrix::Map() but can also be used directly.
*
* \sa Matrix::Map()
*/
template<typename MatrixType, int _PacketAccess>
struct ei_traits<Map<MatrixType, _PacketAccess> > : public ei_traits<MatrixType>
{
enum {
PacketAccess = _PacketAccess,
Flags = ei_traits<MatrixType>::Flags & ~AlignedBit
};
typedef typename ei_meta_if<int(PacketAccess)==ForceAligned,
Map<MatrixType, _PacketAccess>&,
Map<MatrixType, ForceAligned> >::ret AlignedDerivedType;
};
template<typename MatrixType, int PacketAccess> class Map
: public MapBase<Map<MatrixType, PacketAccess> >
{
public:
_EIGEN_GENERIC_PUBLIC_INTERFACE(Map, MapBase<Map>)
typedef typename ei_traits<Map>::AlignedDerivedType AlignedDerivedType;
inline int stride() const { return this->innerSize(); }
AlignedDerivedType _convertToForceAligned()
{
return Map<MatrixType,ForceAligned>(Base::m_data, Base::m_rows.value(), Base::m_cols.value());
}
inline Map(const Scalar* data) : Base(data) {}
inline Map(const Scalar* data, int size) : Base(data, size) {}
inline Map(const Scalar* data, int rows, int cols) : Base(data, rows, cols) {}
inline void resize(int rows, int cols)
{
EIGEN_ONLY_USED_FOR_DEBUG(rows);
EIGEN_ONLY_USED_FOR_DEBUG(cols);
ei_assert(rows == this->rows());
ei_assert(cols == this->cols());
}
inline void resize(int size)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatrixType)
EIGEN_ONLY_USED_FOR_DEBUG(size);
ei_assert(size == this->size());
}
EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
};
/** Constructor copying an existing array of data.
* Only for fixed-size matrices and vectors.
* \param data The array of data to copy
*
* \sa Matrix::Map(const Scalar *)
*/
template<typename _Scalar, int _Rows, int _Cols, int _StorageOrder, int _MaxRows, int _MaxCols>
inline Matrix<_Scalar, _Rows, _Cols, _StorageOrder, _MaxRows, _MaxCols>
::Matrix(const Scalar *data)
{
_set_noalias(Eigen::Map<Matrix>(data));
}
#endif // EIGEN_MAP_H

202
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/MapBase.h Normal file
View File

@ -0,0 +1,202 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_MAPBASE_H
#define EIGEN_MAPBASE_H
/** \class MapBase
*
* \brief Base class for Map and Block expression with direct access
*
* Expression classes inheriting MapBase must define the constant \c PacketAccess,
* and type \c AlignedDerivedType in their respective ei_traits<> specialization structure.
* The value of \c PacketAccess can be either:
* - \b ForceAligned which enforces both aligned loads and stores
* - \b AsRequested which is the default behavior
* The type \c AlignedDerivedType should correspond to the equivalent expression type
* with \c PacketAccess being \c ForceAligned.
*
* \sa class Map, class Block
*/
template<typename Derived> class MapBase
: public MatrixBase<Derived>
{
public:
typedef MatrixBase<Derived> Base;
enum {
IsRowMajor = (int(ei_traits<Derived>::Flags) & RowMajorBit) ? 1 : 0,
PacketAccess = ei_traits<Derived>::PacketAccess,
RowsAtCompileTime = ei_traits<Derived>::RowsAtCompileTime,
ColsAtCompileTime = ei_traits<Derived>::ColsAtCompileTime,
SizeAtCompileTime = Base::SizeAtCompileTime
};
typedef typename ei_traits<Derived>::AlignedDerivedType AlignedDerivedType;
typedef typename ei_traits<Derived>::Scalar Scalar;
typedef typename Base::PacketScalar PacketScalar;
using Base::derived;
inline int rows() const { return m_rows.value(); }
inline int cols() const { return m_cols.value(); }
inline int stride() const { return derived().stride(); }
inline const Scalar* data() const { return m_data; }
template<bool IsForceAligned,typename Dummy> struct force_aligned_impl {
static AlignedDerivedType run(MapBase& a) { return a.derived(); }
};
template<typename Dummy> struct force_aligned_impl<false,Dummy> {
static AlignedDerivedType run(MapBase& a) { return a.derived()._convertToForceAligned(); }
};
/** \returns an expression equivalent to \c *this but having the \c PacketAccess constant
* set to \c ForceAligned. Must be reimplemented by the derived class. */
AlignedDerivedType forceAligned()
{
return force_aligned_impl<int(PacketAccess)==int(ForceAligned),Derived>::run(*this);
}
inline const Scalar& coeff(int row, int col) const
{
if(IsRowMajor)
return m_data[col + row * stride()];
else // column-major
return m_data[row + col * stride()];
}
inline Scalar& coeffRef(int row, int col)
{
if(IsRowMajor)
return const_cast<Scalar*>(m_data)[col + row * stride()];
else // column-major
return const_cast<Scalar*>(m_data)[row + col * stride()];
}
inline const Scalar coeff(int index) const
{
ei_assert(Derived::IsVectorAtCompileTime || (ei_traits<Derived>::Flags & LinearAccessBit));
if ( ((RowsAtCompileTime == 1) == IsRowMajor) || !int(Derived::IsVectorAtCompileTime) )
return m_data[index];
else
return m_data[index*stride()];
}
inline Scalar& coeffRef(int index)
{
ei_assert(Derived::IsVectorAtCompileTime || (ei_traits<Derived>::Flags & LinearAccessBit));
if ( ((RowsAtCompileTime == 1) == IsRowMajor) || !int(Derived::IsVectorAtCompileTime) )
return const_cast<Scalar*>(m_data)[index];
else
return const_cast<Scalar*>(m_data)[index*stride()];
}
template<int LoadMode>
inline PacketScalar packet(int row, int col) const
{
return ei_ploadt<Scalar, int(PacketAccess) == ForceAligned ? Aligned : LoadMode>
(m_data + (IsRowMajor ? col + row * stride()
: row + col * stride()));
}
template<int LoadMode>
inline PacketScalar packet(int index) const
{
return ei_ploadt<Scalar, int(PacketAccess) == ForceAligned ? Aligned : LoadMode>(m_data + index);
}
template<int StoreMode>
inline void writePacket(int row, int col, const PacketScalar& x)
{
ei_pstoret<Scalar, PacketScalar, int(PacketAccess) == ForceAligned ? Aligned : StoreMode>
(const_cast<Scalar*>(m_data) + (IsRowMajor ? col + row * stride()
: row + col * stride()), x);
}
template<int StoreMode>
inline void writePacket(int index, const PacketScalar& x)
{
ei_pstoret<Scalar, PacketScalar, int(PacketAccess) == ForceAligned ? Aligned : StoreMode>
(const_cast<Scalar*>(m_data) + index, x);
}
inline MapBase(const Scalar* data) : m_data(data), m_rows(RowsAtCompileTime), m_cols(ColsAtCompileTime)
{
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
}
inline MapBase(const Scalar* data, int size)
: m_data(data),
m_rows(RowsAtCompileTime == Dynamic ? size : RowsAtCompileTime),
m_cols(ColsAtCompileTime == Dynamic ? size : ColsAtCompileTime)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
ei_assert(size > 0 || data == 0);
ei_assert(SizeAtCompileTime == Dynamic || SizeAtCompileTime == size);
}
inline MapBase(const Scalar* data, int rows, int cols)
: m_data(data), m_rows(rows), m_cols(cols)
{
ei_assert( (data == 0)
|| ( rows > 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows)
&& cols > 0 && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols)));
}
Derived& operator=(const MapBase& other)
{
return Base::operator=(other);
}
template<typename OtherDerived>
Derived& operator=(const MatrixBase<OtherDerived>& other)
{
return Base::operator=(other);
}
using Base::operator*=;
template<typename OtherDerived>
Derived& operator+=(const MatrixBase<OtherDerived>& other)
{ return derived() = forceAligned() + other; }
template<typename OtherDerived>
Derived& operator-=(const MatrixBase<OtherDerived>& other)
{ return derived() = forceAligned() - other; }
Derived& operator*=(const Scalar& other)
{ return derived() = forceAligned() * other; }
Derived& operator/=(const Scalar& other)
{ return derived() = forceAligned() / other; }
protected:
const Scalar* EIGEN_RESTRICT m_data;
const ei_int_if_dynamic<RowsAtCompileTime> m_rows;
const ei_int_if_dynamic<ColsAtCompileTime> m_cols;
};
#endif // EIGEN_MAPBASE_H

295
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/MathFunctions.h Normal file
View File

@ -0,0 +1,295 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_MATHFUNCTIONS_H
#define EIGEN_MATHFUNCTIONS_H
template<typename T> inline typename NumTraits<T>::Real precision();
template<typename T> inline typename NumTraits<T>::Real machine_epsilon();
template<typename T> inline T ei_random(T a, T b);
template<typename T> inline T ei_random();
template<typename T> inline T ei_random_amplitude()
{
if(NumTraits<T>::HasFloatingPoint) return static_cast<T>(1);
else return static_cast<T>(10);
}
/**************
*** int ***
**************/
template<> inline int precision<int>() { return 0; }
template<> inline int machine_epsilon<int>() { return 0; }
inline int ei_real(int x) { return x; }
inline int ei_imag(int) { return 0; }
inline int ei_conj(int x) { return x; }
inline int ei_abs(int x) { return std::abs(x); }
inline int ei_abs2(int x) { return x*x; }
inline int ei_sqrt(int) { ei_assert(false); return 0; }
inline int ei_exp(int) { ei_assert(false); return 0; }
inline int ei_log(int) { ei_assert(false); return 0; }
inline int ei_sin(int) { ei_assert(false); return 0; }
inline int ei_cos(int) { ei_assert(false); return 0; }
inline int ei_atan2(int, int) { ei_assert(false); return 0; }
inline int ei_pow(int x, int y) { return int(std::pow(double(x), y)); }
template<> inline int ei_random(int a, int b)
{
// We can't just do rand()%n as only the high-order bits are really random
return a + static_cast<int>((b-a+1) * (std::rand() / (RAND_MAX + 1.0)));
}
template<> inline int ei_random()
{
return ei_random<int>(-ei_random_amplitude<int>(), ei_random_amplitude<int>());
}
inline bool ei_isMuchSmallerThan(int a, int, int = precision<int>())
{
return a == 0;
}
inline bool ei_isApprox(int a, int b, int = precision<int>())
{
return a == b;
}
inline bool ei_isApproxOrLessThan(int a, int b, int = precision<int>())
{
return a <= b;
}
/**************
*** float ***
**************/
template<> inline float precision<float>() { return 1e-5f; }
template<> inline float machine_epsilon<float>() { return 1.192e-07f; }
inline float ei_real(float x) { return x; }
inline float ei_imag(float) { return 0.f; }
inline float ei_conj(float x) { return x; }
inline float ei_abs(float x) { return std::abs(x); }
inline float ei_abs2(float x) { return x*x; }
inline float ei_sqrt(float x) { return std::sqrt(x); }
inline float ei_exp(float x) { return std::exp(x); }
inline float ei_log(float x) { return std::log(x); }
inline float ei_sin(float x) { return std::sin(x); }
inline float ei_cos(float x) { return std::cos(x); }
inline float ei_atan2(float y, float x) { return std::atan2(y,x); }
inline float ei_pow(float x, float y) { return std::pow(x, y); }
template<> inline float ei_random(float a, float b)
{
#ifdef EIGEN_NICE_RANDOM
int i;
do { i = ei_random<int>(256*int(a),256*int(b));
} while(i==0);
return float(i)/256.f;
#else
return a + (b-a) * float(std::rand()) / float(RAND_MAX);
#endif
}
template<> inline float ei_random()
{
return ei_random<float>(-ei_random_amplitude<float>(), ei_random_amplitude<float>());
}
inline bool ei_isMuchSmallerThan(float a, float b, float prec = precision<float>())
{
return ei_abs(a) <= ei_abs(b) * prec;
}
inline bool ei_isApprox(float a, float b, float prec = precision<float>())
{
return ei_abs(a - b) <= std::min(ei_abs(a), ei_abs(b)) * prec;
}
inline bool ei_isApproxOrLessThan(float a, float b, float prec = precision<float>())
{
return a <= b || ei_isApprox(a, b, prec);
}
/**************
*** double ***
**************/
template<> inline double precision<double>() { return 1e-11; }
template<> inline double machine_epsilon<double>() { return 2.220e-16; }
inline double ei_real(double x) { return x; }
inline double ei_imag(double) { return 0.; }
inline double ei_conj(double x) { return x; }
inline double ei_abs(double x) { return std::abs(x); }
inline double ei_abs2(double x) { return x*x; }
inline double ei_sqrt(double x) { return std::sqrt(x); }
inline double ei_exp(double x) { return std::exp(x); }
inline double ei_log(double x) { return std::log(x); }
inline double ei_sin(double x) { return std::sin(x); }
inline double ei_cos(double x) { return std::cos(x); }
inline double ei_atan2(double y, double x) { return std::atan2(y,x); }
inline double ei_pow(double x, double y) { return std::pow(x, y); }
template<> inline double ei_random(double a, double b)
{
#ifdef EIGEN_NICE_RANDOM
int i;
do { i= ei_random<int>(256*int(a),256*int(b));
} while(i==0);
return i/256.;
#else
return a + (b-a) * std::rand() / RAND_MAX;
#endif
}
template<> inline double ei_random()
{
return ei_random<double>(-ei_random_amplitude<double>(), ei_random_amplitude<double>());
}
inline bool ei_isMuchSmallerThan(double a, double b, double prec = precision<double>())
{
return ei_abs(a) <= ei_abs(b) * prec;
}
inline bool ei_isApprox(double a, double b, double prec = precision<double>())
{
return ei_abs(a - b) <= std::min(ei_abs(a), ei_abs(b)) * prec;
}
inline bool ei_isApproxOrLessThan(double a, double b, double prec = precision<double>())
{
return a <= b || ei_isApprox(a, b, prec);
}
/*********************
*** complex<float> ***
*********************/
template<> inline float precision<std::complex<float> >() { return precision<float>(); }
template<> inline float machine_epsilon<std::complex<float> >() { return machine_epsilon<float>(); }
inline float ei_real(const std::complex<float>& x) { return std::real(x); }
inline float ei_imag(const std::complex<float>& x) { return std::imag(x); }
inline std::complex<float> ei_conj(const std::complex<float>& x) { return std::conj(x); }
inline float ei_abs(const std::complex<float>& x) { return std::abs(x); }
inline float ei_abs2(const std::complex<float>& x) { return std::norm(x); }
inline std::complex<float> ei_exp(std::complex<float> x) { return std::exp(x); }
inline std::complex<float> ei_sin(std::complex<float> x) { return std::sin(x); }
inline std::complex<float> ei_cos(std::complex<float> x) { return std::cos(x); }
inline std::complex<float> ei_atan2(std::complex<float>, std::complex<float> ) { ei_assert(false); return 0; }
template<> inline std::complex<float> ei_random()
{
return std::complex<float>(ei_random<float>(), ei_random<float>());
}
inline bool ei_isMuchSmallerThan(const std::complex<float>& a, const std::complex<float>& b, float prec = precision<float>())
{
return ei_abs2(a) <= ei_abs2(b) * prec * prec;
}
inline bool ei_isMuchSmallerThan(const std::complex<float>& a, float b, float prec = precision<float>())
{
return ei_abs2(a) <= ei_abs2(b) * prec * prec;
}
inline bool ei_isApprox(const std::complex<float>& a, const std::complex<float>& b, float prec = precision<float>())
{
return ei_isApprox(ei_real(a), ei_real(b), prec)
&& ei_isApprox(ei_imag(a), ei_imag(b), prec);
}
// ei_isApproxOrLessThan wouldn't make sense for complex numbers
/**********************
*** complex<double> ***
**********************/
template<> inline double precision<std::complex<double> >() { return precision<double>(); }
template<> inline double machine_epsilon<std::complex<double> >() { return machine_epsilon<double>(); }
inline double ei_real(const std::complex<double>& x) { return std::real(x); }
inline double ei_imag(const std::complex<double>& x) { return std::imag(x); }
inline std::complex<double> ei_conj(const std::complex<double>& x) { return std::conj(x); }
inline double ei_abs(const std::complex<double>& x) { return std::abs(x); }
inline double ei_abs2(const std::complex<double>& x) { return std::norm(x); }
inline std::complex<double> ei_exp(std::complex<double> x) { return std::exp(x); }
inline std::complex<double> ei_sin(std::complex<double> x) { return std::sin(x); }
inline std::complex<double> ei_cos(std::complex<double> x) { return std::cos(x); }
inline std::complex<double> ei_atan2(std::complex<double>, std::complex<double>) { ei_assert(false); return 0; }
template<> inline std::complex<double> ei_random()
{
return std::complex<double>(ei_random<double>(), ei_random<double>());
}
inline bool ei_isMuchSmallerThan(const std::complex<double>& a, const std::complex<double>& b, double prec = precision<double>())
{
return ei_abs2(a) <= ei_abs2(b) * prec * prec;
}
inline bool ei_isMuchSmallerThan(const std::complex<double>& a, double b, double prec = precision<double>())
{
return ei_abs2(a) <= ei_abs2(b) * prec * prec;
}
inline bool ei_isApprox(const std::complex<double>& a, const std::complex<double>& b, double prec = precision<double>())
{
return ei_isApprox(ei_real(a), ei_real(b), prec)
&& ei_isApprox(ei_imag(a), ei_imag(b), prec);
}
// ei_isApproxOrLessThan wouldn't make sense for complex numbers
/******************
*** long double ***
******************/
template<> inline long double precision<long double>() { return precision<double>(); }
template<> inline long double machine_epsilon<long double>() { return 1.084e-19l; }
inline long double ei_real(long double x) { return x; }
inline long double ei_imag(long double) { return 0.; }
inline long double ei_conj(long double x) { return x; }
inline long double ei_abs(long double x) { return std::abs(x); }
inline long double ei_abs2(long double x) { return x*x; }
inline long double ei_sqrt(long double x) { return std::sqrt(x); }
inline long double ei_exp(long double x) { return std::exp(x); }
inline long double ei_log(long double x) { return std::log(x); }
inline long double ei_sin(long double x) { return std::sin(x); }
inline long double ei_cos(long double x) { return std::cos(x); }
inline long double ei_atan2(long double y, long double x) { return std::atan2(y,x); }
inline long double ei_pow(long double x, long double y) { return std::pow(x, y); }
template<> inline long double ei_random(long double a, long double b)
{
return ei_random<double>(static_cast<double>(a),static_cast<double>(b));
}
template<> inline long double ei_random()
{
return ei_random<double>(-ei_random_amplitude<double>(), ei_random_amplitude<double>());
}
inline bool ei_isMuchSmallerThan(long double a, long double b, long double prec = precision<long double>())
{
return ei_abs(a) <= ei_abs(b) * prec;
}
inline bool ei_isApprox(long double a, long double b, long double prec = precision<long double>())
{
return ei_abs(a - b) <= std::min(ei_abs(a), ei_abs(b)) * prec;
}
inline bool ei_isApproxOrLessThan(long double a, long double b, long double prec = precision<long double>())
{
return a <= b || ei_isApprox(a, b, prec);
}
template<typename T> inline T ei_hypot(T x, T y)
{
T _x = ei_abs(x);
T _y = ei_abs(y);
T p = std::max(_x, _y);
T q = std::min(_x, _y);
T qp = q/p;
return p * ei_sqrt(T(1) + qp*qp);
}
#endif // EIGEN_MATHFUNCTIONS_H

663
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Matrix.h Normal file
View File

@ -0,0 +1,663 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_MATRIX_H
#define EIGEN_MATRIX_H
#ifdef EIGEN_INITIALIZE_MATRICES_BY_ZERO
# define EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED for(int i=0;i<base().size();++i) coeffRef(i)=Scalar(0);
#else
# define EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED
#endif
/** \class Matrix
*
* \brief The matrix class, also used for vectors and row-vectors
*
* The %Matrix class is the work-horse for all \em dense (\ref dense "note") matrices and vectors within Eigen.
* Vectors are matrices with one column, and row-vectors are matrices with one row.
*
* The %Matrix class encompasses \em both fixed-size and dynamic-size objects (\ref fixedsize "note").
*
* The first three template parameters are required:
* \param _Scalar Numeric type, i.e. float, double, int
* \param _Rows Number of rows, or \b Dynamic
* \param _Cols Number of columns, or \b Dynamic
*
* The remaining template parameters are optional -- in most cases you don't have to worry about them.
* \param _Options A combination of either \b RowMajor or \b ColMajor, and of either
* \b AutoAlign or \b DontAlign.
* The former controls storage order, and defaults to column-major. The latter controls alignment, which is required
* for vectorization. It defaults to aligning matrices except for fixed sizes that aren't a multiple of the packet size.
* \param _MaxRows Maximum number of rows. Defaults to \a _Rows (\ref maxrows "note").
* \param _MaxCols Maximum number of columns. Defaults to \a _Cols (\ref maxrows "note").
*
* Eigen provides a number of typedefs covering the usual cases. Here are some examples:
*
* \li \c Matrix2d is a 2x2 square matrix of doubles (\c Matrix<double, 2, 2>)
* \li \c Vector4f is a vector of 4 floats (\c Matrix<float, 4, 1>)
* \li \c RowVector3i is a row-vector of 3 ints (\c Matrix<int, 1, 3>)
*
* \li \c MatrixXf is a dynamic-size matrix of floats (\c Matrix<float, Dynamic, Dynamic>)
* \li \c VectorXf is a dynamic-size vector of floats (\c Matrix<float, Dynamic, 1>)
*
* See \link matrixtypedefs this page \endlink for a complete list of predefined \em %Matrix and \em Vector typedefs.
*
* You can access elements of vectors and matrices using normal subscripting:
*
* \code
* Eigen::VectorXd v(10);
* v[0] = 0.1;
* v[1] = 0.2;
* v(0) = 0.3;
* v(1) = 0.4;
*
* Eigen::MatrixXi m(10, 10);
* m(0, 1) = 1;
* m(0, 2) = 2;
* m(0, 3) = 3;
* \endcode
*
* <i><b>Some notes:</b></i>
*
* <dl>
* <dt><b>\anchor dense Dense versus sparse:</b></dt>
* <dd>This %Matrix class handles dense, not sparse matrices and vectors. For sparse matrices and vectors, see the Sparse module.
*
* Dense matrices and vectors are plain usual arrays of coefficients. All the coefficients are stored, in an ordinary contiguous array.
* This is unlike Sparse matrices and vectors where the coefficients are stored as a list of nonzero coefficients.</dd>
*
* <dt><b>\anchor fixedsize Fixed-size versus dynamic-size:</b></dt>
* <dd>Fixed-size means that the numbers of rows and columns are known are compile-time. In this case, Eigen allocates the array
* of coefficients as a fixed-size array, as a class member. This makes sense for very small matrices, typically up to 4x4, sometimes up
* to 16x16. Larger matrices should be declared as dynamic-size even if one happens to know their size at compile-time.
*
* Dynamic-size means that the numbers of rows or columns are not necessarily known at compile-time. In this case they are runtime
* variables, and the array of coefficients is allocated dynamically on the heap.
*
* Note that \em dense matrices, be they Fixed-size or Dynamic-size, <em>do not</em> expand dynamically in the sense of a std::map.
* If you want this behavior, see the Sparse module.</dd>
*
* <dt><b>\anchor maxrows _MaxRows and _MaxCols:</b></dt>
* <dd>In most cases, one just leaves these parameters to the default values.
* These parameters mean the maximum size of rows and columns that the matrix may have. They are useful in cases
* when the exact numbers of rows and columns are not known are compile-time, but it is known at compile-time that they cannot
* exceed a certain value. This happens when taking dynamic-size blocks inside fixed-size matrices: in this case _MaxRows and _MaxCols
* are the dimensions of the original matrix, while _Rows and _Cols are Dynamic.</dd>
* </dl>
*
* \see MatrixBase for the majority of the API methods for matrices
*/
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
struct ei_traits<Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> >
{
typedef _Scalar Scalar;
enum {
RowsAtCompileTime = _Rows,
ColsAtCompileTime = _Cols,
MaxRowsAtCompileTime = _MaxRows,
MaxColsAtCompileTime = _MaxCols,
Flags = ei_compute_matrix_flags<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::ret,
CoeffReadCost = NumTraits<Scalar>::ReadCost
};
};
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
class Matrix
: public MatrixBase<Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> >
{
public:
EIGEN_GENERIC_PUBLIC_INTERFACE(Matrix)
enum { Options = _Options };
friend class Eigen::Map<Matrix, Unaligned>;
typedef class Eigen::Map<Matrix, Unaligned> UnalignedMapType;
friend class Eigen::Map<Matrix, Aligned>;
typedef class Eigen::Map<Matrix, Aligned> AlignedMapType;
protected:
ei_matrix_storage<Scalar, MaxSizeAtCompileTime, RowsAtCompileTime, ColsAtCompileTime, Options> m_storage;
public:
enum { NeedsToAlign = (Options&AutoAlign) == AutoAlign
&& SizeAtCompileTime!=Dynamic && ((sizeof(Scalar)*SizeAtCompileTime)%16)==0 };
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
Base& base() { return *static_cast<Base*>(this); }
const Base& base() const { return *static_cast<const Base*>(this); }
EIGEN_STRONG_INLINE int rows() const { return m_storage.rows(); }
EIGEN_STRONG_INLINE int cols() const { return m_storage.cols(); }
EIGEN_STRONG_INLINE int stride(void) const
{
if(Flags & RowMajorBit)
return m_storage.cols();
else
return m_storage.rows();
}
EIGEN_STRONG_INLINE const Scalar& coeff(int row, int col) const
{
if(Flags & RowMajorBit)
return m_storage.data()[col + row * m_storage.cols()];
else // column-major
return m_storage.data()[row + col * m_storage.rows()];
}
EIGEN_STRONG_INLINE const Scalar& coeff(int index) const
{
return m_storage.data()[index];
}
EIGEN_STRONG_INLINE Scalar& coeffRef(int row, int col)
{
if(Flags & RowMajorBit)
return m_storage.data()[col + row * m_storage.cols()];
else // column-major
return m_storage.data()[row + col * m_storage.rows()];
}
EIGEN_STRONG_INLINE Scalar& coeffRef(int index)
{
return m_storage.data()[index];
}
template<int LoadMode>
EIGEN_STRONG_INLINE PacketScalar packet(int row, int col) const
{
return ei_ploadt<Scalar, LoadMode>
(m_storage.data() + (Flags & RowMajorBit
? col + row * m_storage.cols()
: row + col * m_storage.rows()));
}
template<int LoadMode>
EIGEN_STRONG_INLINE PacketScalar packet(int index) const
{
return ei_ploadt<Scalar, LoadMode>(m_storage.data() + index);
}
template<int StoreMode>
EIGEN_STRONG_INLINE void writePacket(int row, int col, const PacketScalar& x)
{
ei_pstoret<Scalar, PacketScalar, StoreMode>
(m_storage.data() + (Flags & RowMajorBit
? col + row * m_storage.cols()
: row + col * m_storage.rows()), x);
}
template<int StoreMode>
EIGEN_STRONG_INLINE void writePacket(int index, const PacketScalar& x)
{
ei_pstoret<Scalar, PacketScalar, StoreMode>(m_storage.data() + index, x);
}
/** \returns a const pointer to the data array of this matrix */
EIGEN_STRONG_INLINE const Scalar *data() const
{ return m_storage.data(); }
/** \returns a pointer to the data array of this matrix */
EIGEN_STRONG_INLINE Scalar *data()
{ return m_storage.data(); }
/** Resizes \c *this to a \a rows x \a cols matrix.
*
* Makes sense for dynamic-size matrices only.
*
* If the current number of coefficients of \c *this exactly matches the
* product \a rows * \a cols, then no memory allocation is performed and
* the current values are left unchanged. In all other cases, including
* shrinking, the data is reallocated and all previous values are lost.
*
* \sa resize(int) for vectors.
*/
inline void resize(int rows, int cols)
{
ei_assert((MaxRowsAtCompileTime == Dynamic || MaxRowsAtCompileTime >= rows)
&& (RowsAtCompileTime == Dynamic || RowsAtCompileTime == rows)
&& (MaxColsAtCompileTime == Dynamic || MaxColsAtCompileTime >= cols)
&& (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols));
#ifdef EIGEN_INITIALIZE_MATRICES_BY_ZERO
int size = rows*cols;
bool size_changed = size != this->size();
m_storage.resize(size, rows, cols);
if(size_changed) EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED
#else
m_storage.resize(rows*cols, rows, cols);
#endif
}
/** Resizes \c *this to a vector of length \a size
*
* \sa resize(int,int) for the details.
*/
inline void resize(int size)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Matrix)
ei_assert(SizeAtCompileTime == Dynamic || SizeAtCompileTime == size);
#ifdef EIGEN_INITIALIZE_MATRICES_BY_ZERO
bool size_changed = size != this->size();
#endif
if(RowsAtCompileTime == 1)
m_storage.resize(size, 1, size);
else
m_storage.resize(size, size, 1);
#ifdef EIGEN_INITIALIZE_MATRICES_BY_ZERO
if(size_changed) EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED
#endif
}
/** Copies the value of the expression \a other into \c *this with automatic resizing.
*
* *this might be resized to match the dimensions of \a other. If *this was a null matrix (not already initialized),
* it will be initialized.
*
* Note that copying a row-vector into a vector (and conversely) is allowed.
* The resizing, if any, is then done in the appropriate way so that row-vectors
* remain row-vectors and vectors remain vectors.
*/
template<typename OtherDerived>
EIGEN_STRONG_INLINE Matrix& operator=(const MatrixBase<OtherDerived>& other)
{
return _set(other);
}
/** This is a special case of the templated operator=. Its purpose is to
* prevent a default operator= from hiding the templated operator=.
*/
EIGEN_STRONG_INLINE Matrix& operator=(const Matrix& other)
{
return _set(other);
}
EIGEN_INHERIT_ASSIGNMENT_OPERATOR(Matrix, +=)
EIGEN_INHERIT_ASSIGNMENT_OPERATOR(Matrix, -=)
EIGEN_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Matrix, *=)
EIGEN_INHERIT_SCALAR_ASSIGNMENT_OPERATOR(Matrix, /=)
/** Default constructor.
*
* For fixed-size matrices, does nothing.
*
* For dynamic-size matrices, creates an empty matrix of size 0. Does not allocate any array. Such a matrix
* is called a null matrix. This constructor is the unique way to create null matrices: resizing
* a matrix to 0 is not supported.
*
* \sa resize(int,int)
*/
EIGEN_STRONG_INLINE explicit Matrix() : m_storage()
{
_check_template_params();
EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED
}
#ifndef EIGEN_PARSED_BY_DOXYGEN
/** \internal */
Matrix(ei_constructor_without_unaligned_array_assert)
: m_storage(ei_constructor_without_unaligned_array_assert())
{EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED}
#endif
/** Constructs a vector or row-vector with given dimension. \only_for_vectors
*
* Note that this is only useful for dynamic-size vectors. For fixed-size vectors,
* it is redundant to pass the dimension here, so it makes more sense to use the default
* constructor Matrix() instead.
*/
EIGEN_STRONG_INLINE explicit Matrix(int dim)
: m_storage(dim, RowsAtCompileTime == 1 ? 1 : dim, ColsAtCompileTime == 1 ? 1 : dim)
{
_check_template_params();
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Matrix)
ei_assert(dim > 0);
ei_assert(SizeAtCompileTime == Dynamic || SizeAtCompileTime == dim);
EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED
}
/** This constructor has two very different behaviors, depending on the type of *this.
*
* \li When Matrix is a fixed-size vector type of size 2, this constructor constructs
* an initialized vector. The parameters \a x, \a y are copied into the first and second
* coords of the vector respectively.
* \li Otherwise, this constructor constructs an uninitialized matrix with \a x rows and
* \a y columns. This is useful for dynamic-size matrices. For fixed-size matrices,
* it is redundant to pass these parameters, so one should use the default constructor
* Matrix() instead.
*/
EIGEN_STRONG_INLINE Matrix(int x, int y) : m_storage(x*y, x, y)
{
_check_template_params();
if((RowsAtCompileTime == 1 && ColsAtCompileTime == 2)
|| (RowsAtCompileTime == 2 && ColsAtCompileTime == 1))
{
m_storage.data()[0] = Scalar(x);
m_storage.data()[1] = Scalar(y);
}
else
{
ei_assert(x > 0 && (RowsAtCompileTime == Dynamic || RowsAtCompileTime == x)
&& y > 0 && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == y));
EIGEN_INITIALIZE_BY_ZERO_IF_THAT_OPTION_IS_ENABLED
}
}
/** constructs an initialized 2D vector with given coefficients */
EIGEN_STRONG_INLINE Matrix(const float& x, const float& y)
{
_check_template_params();
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Matrix, 2)
m_storage.data()[0] = x;
m_storage.data()[1] = y;
}
/** constructs an initialized 2D vector with given coefficients */
EIGEN_STRONG_INLINE Matrix(const double& x, const double& y)
{
_check_template_params();
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Matrix, 2)
m_storage.data()[0] = x;
m_storage.data()[1] = y;
}
/** constructs an initialized 3D vector with given coefficients */
EIGEN_STRONG_INLINE Matrix(const Scalar& x, const Scalar& y, const Scalar& z)
{
_check_template_params();
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Matrix, 3)
m_storage.data()[0] = x;
m_storage.data()[1] = y;
m_storage.data()[2] = z;
}
/** constructs an initialized 4D vector with given coefficients */
EIGEN_STRONG_INLINE Matrix(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w)
{
_check_template_params();
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Matrix, 4)
m_storage.data()[0] = x;
m_storage.data()[1] = y;
m_storage.data()[2] = z;
m_storage.data()[3] = w;
}
explicit Matrix(const Scalar *data);
/** Constructor copying the value of the expression \a other */
template<typename OtherDerived>
EIGEN_STRONG_INLINE Matrix(const MatrixBase<OtherDerived>& other)
: m_storage(other.rows() * other.cols(), other.rows(), other.cols())
{
_check_template_params();
_set_noalias(other);
}
/** Copy constructor */
EIGEN_STRONG_INLINE Matrix(const Matrix& other)
: Base(), m_storage(other.rows() * other.cols(), other.rows(), other.cols())
{
_check_template_params();
_set_noalias(other);
}
/** Destructor */
inline ~Matrix() {}
/** Override MatrixBase::swap() since for dynamic-sized matrices of same type it is enough to swap the
* data pointers.
*/
template<typename OtherDerived>
void swap(const MatrixBase<OtherDerived>& other);
/** \name Map
* These are convenience functions returning Map objects.
*
* \warning Do not use MapAligned in the Eigen 2.0. Mapping aligned arrays will be fully
* supported in Eigen 3.0 (already implemented in the development branch)
*
* \see class Map
*/
//@{
inline static const UnalignedMapType Map(const Scalar* data)
{ return UnalignedMapType(data); }
inline static UnalignedMapType Map(Scalar* data)
{ return UnalignedMapType(data); }
inline static const UnalignedMapType Map(const Scalar* data, int size)
{ return UnalignedMapType(data, size); }
inline static UnalignedMapType Map(Scalar* data, int size)
{ return UnalignedMapType(data, size); }
inline static const UnalignedMapType Map(const Scalar* data, int rows, int cols)
{ return UnalignedMapType(data, rows, cols); }
inline static UnalignedMapType Map(Scalar* data, int rows, int cols)
{ return UnalignedMapType(data, rows, cols); }
inline static const AlignedMapType MapAligned(const Scalar* data)
{ return AlignedMapType(data); }
inline static AlignedMapType MapAligned(Scalar* data)
{ return AlignedMapType(data); }
inline static const AlignedMapType MapAligned(const Scalar* data, int size)
{ return AlignedMapType(data, size); }
inline static AlignedMapType MapAligned(Scalar* data, int size)
{ return AlignedMapType(data, size); }
inline static const AlignedMapType MapAligned(const Scalar* data, int rows, int cols)
{ return AlignedMapType(data, rows, cols); }
inline static AlignedMapType MapAligned(Scalar* data, int rows, int cols)
{ return AlignedMapType(data, rows, cols); }
//@}
using Base::setConstant;
Matrix& setConstant(int size, const Scalar& value);
Matrix& setConstant(int rows, int cols, const Scalar& value);
using Base::setZero;
Matrix& setZero(int size);
Matrix& setZero(int rows, int cols);
using Base::setOnes;
Matrix& setOnes(int size);
Matrix& setOnes(int rows, int cols);
using Base::setRandom;
Matrix& setRandom(int size);
Matrix& setRandom(int rows, int cols);
using Base::setIdentity;
Matrix& setIdentity(int rows, int cols);
/////////// Geometry module ///////////
template<typename OtherDerived>
explicit Matrix(const RotationBase<OtherDerived,ColsAtCompileTime>& r);
template<typename OtherDerived>
Matrix& operator=(const RotationBase<OtherDerived,ColsAtCompileTime>& r);
// allow to extend Matrix outside Eigen
#ifdef EIGEN_MATRIX_PLUGIN
#include EIGEN_MATRIX_PLUGIN
#endif
private:
/** \internal Resizes *this in preparation for assigning \a other to it.
* Takes care of doing all the checking that's needed.
*
* Note that copying a row-vector into a vector (and conversely) is allowed.
* The resizing, if any, is then done in the appropriate way so that row-vectors
* remain row-vectors and vectors remain vectors.
*/
template<typename OtherDerived>
EIGEN_STRONG_INLINE void _resize_to_match(const MatrixBase<OtherDerived>& other)
{
if(RowsAtCompileTime == 1)
{
ei_assert(other.isVector());
resize(1, other.size());
}
else if(ColsAtCompileTime == 1)
{
ei_assert(other.isVector());
resize(other.size(), 1);
}
else resize(other.rows(), other.cols());
}
/** \internal Copies the value of the expression \a other into \c *this with automatic resizing.
*
* *this might be resized to match the dimensions of \a other. If *this was a null matrix (not already initialized),
* it will be initialized.
*
* Note that copying a row-vector into a vector (and conversely) is allowed.
* The resizing, if any, is then done in the appropriate way so that row-vectors
* remain row-vectors and vectors remain vectors.
*
* \sa operator=(const MatrixBase<OtherDerived>&), _set_noalias()
*/
template<typename OtherDerived>
EIGEN_STRONG_INLINE Matrix& _set(const MatrixBase<OtherDerived>& other)
{
// this enum introduced to fix compilation with gcc 3.3
enum { cond = int(OtherDerived::Flags) & EvalBeforeAssigningBit };
_set_selector(other.derived(), typename ei_meta_if<bool(cond), ei_meta_true, ei_meta_false>::ret());
return *this;
}
template<typename OtherDerived>
EIGEN_STRONG_INLINE void _set_selector(const OtherDerived& other, const ei_meta_true&) { _set_noalias(other.eval()); }
template<typename OtherDerived>
EIGEN_STRONG_INLINE void _set_selector(const OtherDerived& other, const ei_meta_false&) { _set_noalias(other); }
/** \internal Like _set() but additionally makes the assumption that no aliasing effect can happen (which
* is the case when creating a new matrix) so one can enforce lazy evaluation.
*
* \sa operator=(const MatrixBase<OtherDerived>&), _set()
*/
template<typename OtherDerived>
EIGEN_STRONG_INLINE Matrix& _set_noalias(const MatrixBase<OtherDerived>& other)
{
_resize_to_match(other);
// the 'false' below means to enforce lazy evaluation. We don't use lazyAssign() because
// it wouldn't allow to copy a row-vector into a column-vector.
return ei_assign_selector<Matrix,OtherDerived,false>::run(*this, other.derived());
}
static EIGEN_STRONG_INLINE void _check_template_params()
{
EIGEN_STATIC_ASSERT((_Rows > 0
&& _Cols > 0
&& _MaxRows <= _Rows
&& _MaxCols <= _Cols
&& (_Options & (AutoAlign|RowMajor)) == _Options),
INVALID_MATRIX_TEMPLATE_PARAMETERS)
}
template<typename MatrixType, typename OtherDerived, bool IsSameType, bool IsDynamicSize>
friend struct ei_matrix_swap_impl;
};
template<typename MatrixType, typename OtherDerived,
bool IsSameType = ei_is_same_type<MatrixType, OtherDerived>::ret,
bool IsDynamicSize = MatrixType::SizeAtCompileTime==Dynamic>
struct ei_matrix_swap_impl
{
static inline void run(MatrixType& matrix, MatrixBase<OtherDerived>& other)
{
matrix.base().swap(other);
}
};
template<typename MatrixType, typename OtherDerived>
struct ei_matrix_swap_impl<MatrixType, OtherDerived, true, true>
{
static inline void run(MatrixType& matrix, MatrixBase<OtherDerived>& other)
{
matrix.m_storage.swap(other.derived().m_storage);
}
};
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
template<typename OtherDerived>
inline void Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::swap(const MatrixBase<OtherDerived>& other)
{
// the Eigen:: here is to work around a stupid ICC 11.1 bug.
Eigen::ei_matrix_swap_impl<Matrix, OtherDerived>::run(*this, *const_cast<MatrixBase<OtherDerived>*>(&other));
}
/** \defgroup matrixtypedefs Global matrix typedefs
*
* \ingroup Core_Module
*
* Eigen defines several typedef shortcuts for most common matrix and vector types.
*
* The general patterns are the following:
*
* \c MatrixSizeType where \c Size can be \c 2,\c 3,\c 4 for fixed size square matrices or \c X for dynamic size,
* and where \c Type can be \c i for integer, \c f for float, \c d for double, \c cf for complex float, \c cd
* for complex double.
*
* For example, \c Matrix3d is a fixed-size 3x3 matrix type of doubles, and \c MatrixXf is a dynamic-size matrix of floats.
*
* There are also \c VectorSizeType and \c RowVectorSizeType which are self-explanatory. For example, \c Vector4cf is
* a fixed-size vector of 4 complex floats.
*
* \sa class Matrix
*/
#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \
/** \ingroup matrixtypedefs */ \
typedef Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix; \
/** \ingroup matrixtypedefs */ \
typedef Matrix<Type, Size, 1> Vector##SizeSuffix##TypeSuffix; \
/** \ingroup matrixtypedefs */ \
typedef Matrix<Type, 1, Size> RowVector##SizeSuffix##TypeSuffix;
#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 2, 2) \
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 3, 3) \
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 4, 4) \
EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Dynamic, X)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<float>, cf)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<double>, cd)
#undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
#undef EIGEN_MAKE_TYPEDEFS
#undef EIGEN_MAKE_TYPEDEFS_LARGE
#define EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, SizeSuffix) \
using Eigen::Matrix##SizeSuffix##TypeSuffix; \
using Eigen::Vector##SizeSuffix##TypeSuffix; \
using Eigen::RowVector##SizeSuffix##TypeSuffix;
#define EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(TypeSuffix) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 2) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 3) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, 4) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, X) \
#define EIGEN_USING_MATRIX_TYPEDEFS \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(i) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(f) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(d) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(cf) \
EIGEN_USING_MATRIX_TYPEDEFS_FOR_TYPE(cd)
#endif // EIGEN_MATRIX_H

645
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/MatrixBase.h Normal file
View File

@ -0,0 +1,645 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_MATRIXBASE_H
#define EIGEN_MATRIXBASE_H
/** \class MatrixBase
*
* \brief Base class for all matrices, vectors, and expressions
*
* This class is the base that is inherited by all matrix, vector, and expression
* types. Most of the Eigen API is contained in this class. Other important classes for
* the Eigen API are Matrix, Cwise, and PartialRedux.
*
* Note that some methods are defined in the \ref Array module.
*
* \param Derived is the derived type, e.g. a matrix type, or an expression, etc.
*
* When writing a function taking Eigen objects as argument, if you want your function
* to take as argument any matrix, vector, or expression, just let it take a
* MatrixBase argument. As an example, here is a function printFirstRow which, given
* a matrix, vector, or expression \a x, prints the first row of \a x.
*
* \code
template<typename Derived>
void printFirstRow(const Eigen::MatrixBase<Derived>& x)
{
cout << x.row(0) << endl;
}
* \endcode
*
*/
template<typename Derived> class MatrixBase
{
public:
#ifndef EIGEN_PARSED_BY_DOXYGEN
class InnerIterator;
typedef typename ei_traits<Derived>::Scalar Scalar;
typedef typename ei_packet_traits<Scalar>::type PacketScalar;
#endif // not EIGEN_PARSED_BY_DOXYGEN
enum {
RowsAtCompileTime = ei_traits<Derived>::RowsAtCompileTime,
/**< The number of rows at compile-time. This is just a copy of the value provided
* by the \a Derived type. If a value is not known at compile-time,
* it is set to the \a Dynamic constant.
* \sa MatrixBase::rows(), MatrixBase::cols(), ColsAtCompileTime, SizeAtCompileTime */
ColsAtCompileTime = ei_traits<Derived>::ColsAtCompileTime,
/**< The number of columns at compile-time. This is just a copy of the value provided
* by the \a Derived type. If a value is not known at compile-time,
* it is set to the \a Dynamic constant.
* \sa MatrixBase::rows(), MatrixBase::cols(), RowsAtCompileTime, SizeAtCompileTime */
SizeAtCompileTime = (ei_size_at_compile_time<ei_traits<Derived>::RowsAtCompileTime,
ei_traits<Derived>::ColsAtCompileTime>::ret),
/**< This is equal to the number of coefficients, i.e. the number of
* rows times the number of columns, or to \a Dynamic if this is not
* known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */
MaxRowsAtCompileTime = ei_traits<Derived>::MaxRowsAtCompileTime,
/**< This value is equal to the maximum possible number of rows that this expression
* might have. If this expression might have an arbitrarily high number of rows,
* this value is set to \a Dynamic.
*
* This value is useful to know when evaluating an expression, in order to determine
* whether it is possible to avoid doing a dynamic memory allocation.
*
* \sa RowsAtCompileTime, MaxColsAtCompileTime, MaxSizeAtCompileTime
*/
MaxColsAtCompileTime = ei_traits<Derived>::MaxColsAtCompileTime,
/**< This value is equal to the maximum possible number of columns that this expression
* might have. If this expression might have an arbitrarily high number of columns,
* this value is set to \a Dynamic.
*
* This value is useful to know when evaluating an expression, in order to determine
* whether it is possible to avoid doing a dynamic memory allocation.
*
* \sa ColsAtCompileTime, MaxRowsAtCompileTime, MaxSizeAtCompileTime
*/
MaxSizeAtCompileTime = (ei_size_at_compile_time<ei_traits<Derived>::MaxRowsAtCompileTime,
ei_traits<Derived>::MaxColsAtCompileTime>::ret),
/**< This value is equal to the maximum possible number of coefficients that this expression
* might have. If this expression might have an arbitrarily high number of coefficients,
* this value is set to \a Dynamic.
*
* This value is useful to know when evaluating an expression, in order to determine
* whether it is possible to avoid doing a dynamic memory allocation.
*
* \sa SizeAtCompileTime, MaxRowsAtCompileTime, MaxColsAtCompileTime
*/
IsVectorAtCompileTime = ei_traits<Derived>::RowsAtCompileTime == 1
|| ei_traits<Derived>::ColsAtCompileTime == 1,
/**< This is set to true if either the number of rows or the number of
* columns is known at compile-time to be equal to 1. Indeed, in that case,
* we are dealing with a column-vector (if there is only one column) or with
* a row-vector (if there is only one row). */
Flags = ei_traits<Derived>::Flags,
/**< This stores expression \ref flags flags which may or may not be inherited by new expressions
* constructed from this one. See the \ref flags "list of flags".
*/
CoeffReadCost = ei_traits<Derived>::CoeffReadCost
/**< This is a rough measure of how expensive it is to read one coefficient from
* this expression.
*/
};
#ifndef EIGEN_PARSED_BY_DOXYGEN
/** This is the "real scalar" type; if the \a Scalar type is already real numbers
* (e.g. int, float or double) then \a RealScalar is just the same as \a Scalar. If
* \a Scalar is \a std::complex<T> then RealScalar is \a T.
*
* \sa class NumTraits
*/
typedef typename NumTraits<Scalar>::Real RealScalar;
/** type of the equivalent square matrix */
typedef Matrix<Scalar,EIGEN_ENUM_MAX(RowsAtCompileTime,ColsAtCompileTime),
EIGEN_ENUM_MAX(RowsAtCompileTime,ColsAtCompileTime)> SquareMatrixType;
#endif // not EIGEN_PARSED_BY_DOXYGEN
/** \returns the number of rows. \sa cols(), RowsAtCompileTime */
inline int rows() const { return derived().rows(); }
/** \returns the number of columns. \sa rows(), ColsAtCompileTime*/
inline int cols() const { return derived().cols(); }
/** \returns the number of coefficients, which is \a rows()*cols().
* \sa rows(), cols(), SizeAtCompileTime. */
inline int size() const { return rows() * cols(); }
/** \returns the number of nonzero coefficients which is in practice the number
* of stored coefficients. */
inline int nonZeros() const { return size(); }
/** \returns true if either the number of rows or the number of columns is equal to 1.
* In other words, this function returns
* \code rows()==1 || cols()==1 \endcode
* \sa rows(), cols(), IsVectorAtCompileTime. */
inline bool isVector() const { return rows()==1 || cols()==1; }
/** \returns the size of the storage major dimension,
* i.e., the number of columns for a columns major matrix, and the number of rows otherwise */
int outerSize() const { return (int(Flags)&RowMajorBit) ? this->rows() : this->cols(); }
/** \returns the size of the inner dimension according to the storage order,
* i.e., the number of rows for a columns major matrix, and the number of cols otherwise */
int innerSize() const { return (int(Flags)&RowMajorBit) ? this->cols() : this->rows(); }
#ifndef EIGEN_PARSED_BY_DOXYGEN
/** \internal the plain matrix type corresponding to this expression. Note that is not necessarily
* exactly the return type of eval(): in the case of plain matrices, the return type of eval() is a const
* reference to a matrix, not a matrix! It guaranteed however, that the return type of eval() is either
* PlainMatrixType or const PlainMatrixType&.
*/
typedef typename ei_plain_matrix_type<Derived>::type PlainMatrixType;
/** \internal the column-major plain matrix type corresponding to this expression. Note that is not necessarily
* exactly the return type of eval(): in the case of plain matrices, the return type of eval() is a const
* reference to a matrix, not a matrix!
* The only difference from PlainMatrixType is that PlainMatrixType_ColMajor is guaranteed to be column-major.
*/
typedef typename ei_plain_matrix_type<Derived>::type PlainMatrixType_ColMajor;
/** \internal Represents a matrix with all coefficients equal to one another*/
typedef CwiseNullaryOp<ei_scalar_constant_op<Scalar>,Derived> ConstantReturnType;
/** \internal Represents a scalar multiple of a matrix */
typedef CwiseUnaryOp<ei_scalar_multiple_op<Scalar>, Derived> ScalarMultipleReturnType;
/** \internal Represents a quotient of a matrix by a scalar*/
typedef CwiseUnaryOp<ei_scalar_quotient1_op<Scalar>, Derived> ScalarQuotient1ReturnType;
/** \internal the return type of MatrixBase::conjugate() */
typedef typename ei_meta_if<NumTraits<Scalar>::IsComplex,
const CwiseUnaryOp<ei_scalar_conjugate_op<Scalar>, Derived>,
const Derived&
>::ret ConjugateReturnType;
/** \internal the return type of MatrixBase::real() */
typedef CwiseUnaryOp<ei_scalar_real_op<Scalar>, Derived> RealReturnType;
/** \internal the return type of MatrixBase::imag() */
typedef CwiseUnaryOp<ei_scalar_imag_op<Scalar>, Derived> ImagReturnType;
/** \internal the return type of MatrixBase::adjoint() */
typedef Eigen::Transpose<NestByValue<typename ei_cleantype<ConjugateReturnType>::type> >
AdjointReturnType;
/** \internal the return type of MatrixBase::eigenvalues() */
typedef Matrix<typename NumTraits<typename ei_traits<Derived>::Scalar>::Real, ei_traits<Derived>::ColsAtCompileTime, 1> EigenvaluesReturnType;
/** \internal expression tyepe of a column */
typedef Block<Derived, ei_traits<Derived>::RowsAtCompileTime, 1> ColXpr;
/** \internal expression tyepe of a column */
typedef Block<Derived, 1, ei_traits<Derived>::ColsAtCompileTime> RowXpr;
/** \internal the return type of identity */
typedef CwiseNullaryOp<ei_scalar_identity_op<Scalar>,Derived> IdentityReturnType;
/** \internal the return type of unit vectors */
typedef Block<CwiseNullaryOp<ei_scalar_identity_op<Scalar>, SquareMatrixType>,
ei_traits<Derived>::RowsAtCompileTime,
ei_traits<Derived>::ColsAtCompileTime> BasisReturnType;
#endif // not EIGEN_PARSED_BY_DOXYGEN
/** Copies \a other into *this. \returns a reference to *this. */
template<typename OtherDerived>
Derived& operator=(const MatrixBase<OtherDerived>& other);
/** Special case of the template operator=, in order to prevent the compiler
* from generating a default operator= (issue hit with g++ 4.1)
*/
inline Derived& operator=(const MatrixBase& other)
{
return this->operator=<Derived>(other);
}
#ifndef EIGEN_PARSED_BY_DOXYGEN
/** Copies \a other into *this without evaluating other. \returns a reference to *this. */
template<typename OtherDerived>
Derived& lazyAssign(const MatrixBase<OtherDerived>& other);
/** Overloaded for cache friendly product evaluation */
template<typename Lhs, typename Rhs>
Derived& lazyAssign(const Product<Lhs,Rhs,CacheFriendlyProduct>& product);
/** Overloaded for cache friendly product evaluation */
template<typename OtherDerived>
Derived& lazyAssign(const Flagged<OtherDerived, 0, EvalBeforeNestingBit | EvalBeforeAssigningBit>& other)
{ return lazyAssign(other._expression()); }
#endif // not EIGEN_PARSED_BY_DOXYGEN
CommaInitializer<Derived> operator<< (const Scalar& s);
template<typename OtherDerived>
CommaInitializer<Derived> operator<< (const MatrixBase<OtherDerived>& other);
const Scalar coeff(int row, int col) const;
const Scalar operator()(int row, int col) const;
Scalar& coeffRef(int row, int col);
Scalar& operator()(int row, int col);
const Scalar coeff(int index) const;
const Scalar operator[](int index) const;
const Scalar operator()(int index) const;
Scalar& coeffRef(int index);
Scalar& operator[](int index);
Scalar& operator()(int index);
#ifndef EIGEN_PARSED_BY_DOXYGEN
template<typename OtherDerived>
void copyCoeff(int row, int col, const MatrixBase<OtherDerived>& other);
template<typename OtherDerived>
void copyCoeff(int index, const MatrixBase<OtherDerived>& other);
template<typename OtherDerived, int StoreMode, int LoadMode>
void copyPacket(int row, int col, const MatrixBase<OtherDerived>& other);
template<typename OtherDerived, int StoreMode, int LoadMode>
void copyPacket(int index, const MatrixBase<OtherDerived>& other);
#endif // not EIGEN_PARSED_BY_DOXYGEN
template<int LoadMode>
PacketScalar packet(int row, int col) const;
template<int StoreMode>
void writePacket(int row, int col, const PacketScalar& x);
template<int LoadMode>
PacketScalar packet(int index) const;
template<int StoreMode>
void writePacket(int index, const PacketScalar& x);
const Scalar x() const;
const Scalar y() const;
const Scalar z() const;
const Scalar w() const;
Scalar& x();
Scalar& y();
Scalar& z();
Scalar& w();
const CwiseUnaryOp<ei_scalar_opposite_op<typename ei_traits<Derived>::Scalar>,Derived> operator-() const;
template<typename OtherDerived>
const CwiseBinaryOp<ei_scalar_sum_op<typename ei_traits<Derived>::Scalar>, Derived, OtherDerived>
operator+(const MatrixBase<OtherDerived> &other) const;
template<typename OtherDerived>
const CwiseBinaryOp<ei_scalar_difference_op<typename ei_traits<Derived>::Scalar>, Derived, OtherDerived>
operator-(const MatrixBase<OtherDerived> &other) const;
template<typename OtherDerived>
Derived& operator+=(const MatrixBase<OtherDerived>& other);
template<typename OtherDerived>
Derived& operator-=(const MatrixBase<OtherDerived>& other);
template<typename Lhs,typename Rhs>
Derived& operator+=(const Flagged<Product<Lhs,Rhs,CacheFriendlyProduct>, 0, EvalBeforeNestingBit | EvalBeforeAssigningBit>& other);
Derived& operator*=(const Scalar& other);
Derived& operator/=(const Scalar& other);
const ScalarMultipleReturnType operator*(const Scalar& scalar) const;
const CwiseUnaryOp<ei_scalar_quotient1_op<typename ei_traits<Derived>::Scalar>, Derived>
operator/(const Scalar& scalar) const;
inline friend const CwiseUnaryOp<ei_scalar_multiple_op<typename ei_traits<Derived>::Scalar>, Derived>
operator*(const Scalar& scalar, const MatrixBase& matrix)
{ return matrix*scalar; }
template<typename OtherDerived>
const typename ProductReturnType<Derived,OtherDerived>::Type
operator*(const MatrixBase<OtherDerived> &other) const;
template<typename OtherDerived>
Derived& operator*=(const MatrixBase<OtherDerived>& other);
template<typename OtherDerived>
typename ei_plain_matrix_type_column_major<OtherDerived>::type
solveTriangular(const MatrixBase<OtherDerived>& other) const;
template<typename OtherDerived>
void solveTriangularInPlace(const MatrixBase<OtherDerived>& other) const;
template<typename OtherDerived>
Scalar dot(const MatrixBase<OtherDerived>& other) const;
RealScalar squaredNorm() const;
RealScalar norm() const;
const PlainMatrixType normalized() const;
void normalize();
Eigen::Transpose<Derived> transpose();
const Eigen::Transpose<Derived> transpose() const;
void transposeInPlace();
const AdjointReturnType adjoint() const;
RowXpr row(int i);
const RowXpr row(int i) const;
ColXpr col(int i);
const ColXpr col(int i) const;
Minor<Derived> minor(int row, int col);
const Minor<Derived> minor(int row, int col) const;
typename BlockReturnType<Derived>::Type block(int startRow, int startCol, int blockRows, int blockCols);
const typename BlockReturnType<Derived>::Type
block(int startRow, int startCol, int blockRows, int blockCols) const;
typename BlockReturnType<Derived>::SubVectorType segment(int start, int size);
const typename BlockReturnType<Derived>::SubVectorType segment(int start, int size) const;
typename BlockReturnType<Derived,Dynamic>::SubVectorType start(int size);
const typename BlockReturnType<Derived,Dynamic>::SubVectorType start(int size) const;
typename BlockReturnType<Derived,Dynamic>::SubVectorType end(int size);
const typename BlockReturnType<Derived,Dynamic>::SubVectorType end(int size) const;
typename BlockReturnType<Derived>::Type corner(CornerType type, int cRows, int cCols);
const typename BlockReturnType<Derived>::Type corner(CornerType type, int cRows, int cCols) const;
template<int BlockRows, int BlockCols>
typename BlockReturnType<Derived, BlockRows, BlockCols>::Type block(int startRow, int startCol);
template<int BlockRows, int BlockCols>
const typename BlockReturnType<Derived, BlockRows, BlockCols>::Type block(int startRow, int startCol) const;
template<int CRows, int CCols>
typename BlockReturnType<Derived, CRows, CCols>::Type corner(CornerType type);
template<int CRows, int CCols>
const typename BlockReturnType<Derived, CRows, CCols>::Type corner(CornerType type) const;
template<int Size> typename BlockReturnType<Derived,Size>::SubVectorType start(void);
template<int Size> const typename BlockReturnType<Derived,Size>::SubVectorType start() const;
template<int Size> typename BlockReturnType<Derived,Size>::SubVectorType end();
template<int Size> const typename BlockReturnType<Derived,Size>::SubVectorType end() const;
template<int Size> typename BlockReturnType<Derived,Size>::SubVectorType segment(int start);
template<int Size> const typename BlockReturnType<Derived,Size>::SubVectorType segment(int start) const;
DiagonalCoeffs<Derived> diagonal();
const DiagonalCoeffs<Derived> diagonal() const;
template<unsigned int Mode> Part<Derived, Mode> part();
template<unsigned int Mode> const Part<Derived, Mode> part() const;
static const ConstantReturnType
Constant(int rows, int cols, const Scalar& value);
static const ConstantReturnType
Constant(int size, const Scalar& value);
static const ConstantReturnType
Constant(const Scalar& value);
template<typename CustomNullaryOp>
static const CwiseNullaryOp<CustomNullaryOp, Derived>
NullaryExpr(int rows, int cols, const CustomNullaryOp& func);
template<typename CustomNullaryOp>
static const CwiseNullaryOp<CustomNullaryOp, Derived>
NullaryExpr(int size, const CustomNullaryOp& func);
template<typename CustomNullaryOp>
static const CwiseNullaryOp<CustomNullaryOp, Derived>
NullaryExpr(const CustomNullaryOp& func);
static const ConstantReturnType Zero(int rows, int cols);
static const ConstantReturnType Zero(int size);
static const ConstantReturnType Zero();
static const ConstantReturnType Ones(int rows, int cols);
static const ConstantReturnType Ones(int size);
static const ConstantReturnType Ones();
static const IdentityReturnType Identity();
static const IdentityReturnType Identity(int rows, int cols);
static const BasisReturnType Unit(int size, int i);
static const BasisReturnType Unit(int i);
static const BasisReturnType UnitX();
static const BasisReturnType UnitY();
static const BasisReturnType UnitZ();
static const BasisReturnType UnitW();
const DiagonalMatrix<Derived> asDiagonal() const;
void fill(const Scalar& value);
Derived& setConstant(const Scalar& value);
Derived& setZero();
Derived& setOnes();
Derived& setRandom();
Derived& setIdentity();
template<typename OtherDerived>
bool isApprox(const MatrixBase<OtherDerived>& other,
RealScalar prec = precision<Scalar>()) const;
bool isMuchSmallerThan(const RealScalar& other,
RealScalar prec = precision<Scalar>()) const;
template<typename OtherDerived>
bool isMuchSmallerThan(const MatrixBase<OtherDerived>& other,
RealScalar prec = precision<Scalar>()) const;
bool isApproxToConstant(const Scalar& value, RealScalar prec = precision<Scalar>()) const;
bool isConstant(const Scalar& value, RealScalar prec = precision<Scalar>()) const;
bool isZero(RealScalar prec = precision<Scalar>()) const;
bool isOnes(RealScalar prec = precision<Scalar>()) const;
bool isIdentity(RealScalar prec = precision<Scalar>()) const;
bool isDiagonal(RealScalar prec = precision<Scalar>()) const;
bool isUpperTriangular(RealScalar prec = precision<Scalar>()) const;
bool isLowerTriangular(RealScalar prec = precision<Scalar>()) const;
template<typename OtherDerived>
bool isOrthogonal(const MatrixBase<OtherDerived>& other,
RealScalar prec = precision<Scalar>()) const;
bool isUnitary(RealScalar prec = precision<Scalar>()) const;
template<typename OtherDerived>
inline bool operator==(const MatrixBase<OtherDerived>& other) const
{ return (cwise() == other).all(); }
template<typename OtherDerived>
inline bool operator!=(const MatrixBase<OtherDerived>& other) const
{ return (cwise() != other).any(); }
template<typename NewType>
const CwiseUnaryOp<ei_scalar_cast_op<typename ei_traits<Derived>::Scalar, NewType>, Derived> cast() const;
/** \returns the matrix or vector obtained by evaluating this expression.
*
* Notice that in the case of a plain matrix or vector (not an expression) this function just returns
* a const reference, in order to avoid a useless copy.
*/
EIGEN_STRONG_INLINE const typename ei_eval<Derived>::type eval() const
{ return typename ei_eval<Derived>::type(derived()); }
template<typename OtherDerived>
void swap(const MatrixBase<OtherDerived>& other);
template<unsigned int Added>
const Flagged<Derived, Added, 0> marked() const;
const Flagged<Derived, 0, EvalBeforeNestingBit | EvalBeforeAssigningBit> lazy() const;
/** \returns number of elements to skip to pass from one row (resp. column) to another
* for a row-major (resp. column-major) matrix.
* Combined with coeffRef() and the \ref flags flags, it allows a direct access to the data
* of the underlying matrix.
*/
inline int stride(void) const { return derived().stride(); }
inline const NestByValue<Derived> nestByValue() const;
ConjugateReturnType conjugate() const;
const RealReturnType real() const;
const ImagReturnType imag() const;
template<typename CustomUnaryOp>
const CwiseUnaryOp<CustomUnaryOp, Derived> unaryExpr(const CustomUnaryOp& func = CustomUnaryOp()) const;
template<typename CustomBinaryOp, typename OtherDerived>
const CwiseBinaryOp<CustomBinaryOp, Derived, OtherDerived>
binaryExpr(const MatrixBase<OtherDerived> &other, const CustomBinaryOp& func = CustomBinaryOp()) const;
Scalar sum() const;
Scalar trace() const;
typename ei_traits<Derived>::Scalar minCoeff() const;
typename ei_traits<Derived>::Scalar maxCoeff() const;
typename ei_traits<Derived>::Scalar minCoeff(int* row, int* col) const;
typename ei_traits<Derived>::Scalar maxCoeff(int* row, int* col) const;
typename ei_traits<Derived>::Scalar minCoeff(int* index) const;
typename ei_traits<Derived>::Scalar maxCoeff(int* index) const;
template<typename BinaryOp>
typename ei_result_of<BinaryOp(typename ei_traits<Derived>::Scalar)>::type
redux(const BinaryOp& func) const;
template<typename Visitor>
void visit(Visitor& func) const;
#ifndef EIGEN_PARSED_BY_DOXYGEN
inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
inline Derived& derived() { return *static_cast<Derived*>(this); }
inline Derived& const_cast_derived() const
{ return *static_cast<Derived*>(const_cast<MatrixBase*>(this)); }
#endif // not EIGEN_PARSED_BY_DOXYGEN
const Cwise<Derived> cwise() const;
Cwise<Derived> cwise();
inline const WithFormat<Derived> format(const IOFormat& fmt) const;
/////////// Array module ///////////
bool all(void) const;
bool any(void) const;
int count() const;
const PartialRedux<Derived,Horizontal> rowwise() const;
const PartialRedux<Derived,Vertical> colwise() const;
static const CwiseNullaryOp<ei_scalar_random_op<Scalar>,Derived> Random(int rows, int cols);
static const CwiseNullaryOp<ei_scalar_random_op<Scalar>,Derived> Random(int size);
static const CwiseNullaryOp<ei_scalar_random_op<Scalar>,Derived> Random();
template<typename ThenDerived,typename ElseDerived>
const Select<Derived,ThenDerived,ElseDerived>
select(const MatrixBase<ThenDerived>& thenMatrix,
const MatrixBase<ElseDerived>& elseMatrix) const;
template<typename ThenDerived>
inline const Select<Derived,ThenDerived, NestByValue<typename ThenDerived::ConstantReturnType> >
select(const MatrixBase<ThenDerived>& thenMatrix, typename ThenDerived::Scalar elseScalar) const;
template<typename ElseDerived>
inline const Select<Derived, NestByValue<typename ElseDerived::ConstantReturnType>, ElseDerived >
select(typename ElseDerived::Scalar thenScalar, const MatrixBase<ElseDerived>& elseMatrix) const;
template<int p> RealScalar lpNorm() const;
/////////// LU module ///////////
const LU<PlainMatrixType> lu() const;
const PlainMatrixType inverse() const;
template<typename ResultType>
void computeInverse(MatrixBase<ResultType> *result) const;
Scalar determinant() const;
/////////// Cholesky module ///////////
const LLT<PlainMatrixType> llt() const;
const LDLT<PlainMatrixType> ldlt() const;
/////////// QR module ///////////
const QR<PlainMatrixType> qr() const;
EigenvaluesReturnType eigenvalues() const;
RealScalar operatorNorm() const;
/////////// SVD module ///////////
SVD<PlainMatrixType> svd() const;
/////////// Geometry module ///////////
template<typename OtherDerived>
PlainMatrixType cross(const MatrixBase<OtherDerived>& other) const;
PlainMatrixType unitOrthogonal(void) const;
Matrix<Scalar,3,1> eulerAngles(int a0, int a1, int a2) const;
/////////// Sparse module ///////////
// dense = spasre * dense
template<typename Derived1, typename Derived2>
Derived& lazyAssign(const SparseProduct<Derived1,Derived2,SparseTimeDenseProduct>& product);
// dense = dense * spasre
template<typename Derived1, typename Derived2>
Derived& lazyAssign(const SparseProduct<Derived1,Derived2,DenseTimeSparseProduct>& product);
#ifdef EIGEN_MATRIXBASE_PLUGIN
#include EIGEN_MATRIXBASE_PLUGIN
#endif
protected:
/** Default constructor. Do nothing. */
MatrixBase()
{
/* Just checks for self-consistency of the flags.
* Only do it when debugging Eigen, as this borders on paranoiac and could slow compilation down
*/
#ifdef EIGEN_INTERNAL_DEBUGGING
EIGEN_STATIC_ASSERT(ei_are_flags_consistent<Flags>::ret,
INVALID_MATRIXBASE_TEMPLATE_PARAMETERS)
#endif
}
private:
explicit MatrixBase(int);
MatrixBase(int,int);
template<typename OtherDerived> explicit MatrixBase(const MatrixBase<OtherDerived>&);
};
#endif // EIGEN_MATRIXBASE_H

249
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/MatrixStorage.h Normal file
View File

@ -0,0 +1,249 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2006-2009 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_MATRIXSTORAGE_H
#define EIGEN_MATRIXSTORAGE_H
struct ei_constructor_without_unaligned_array_assert {};
/** \internal
* Static array automatically aligned if the total byte size is a multiple of 16 and the matrix options require auto alignment
*/
template <typename T, int Size, int MatrixOptions,
bool Align = (MatrixOptions&AutoAlign) && (((Size*sizeof(T))&0xf)==0)
> struct ei_matrix_array
{
EIGEN_ALIGN_128 T array[Size];
ei_matrix_array()
{
#ifndef EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
ei_assert((reinterpret_cast<size_t>(array) & 0xf) == 0
&& "this assertion is explained here: http://eigen.tuxfamily.org/dox/UnalignedArrayAssert.html **** READ THIS WEB PAGE !!! ****");
#endif
}
ei_matrix_array(ei_constructor_without_unaligned_array_assert) {}
};
template <typename T, int Size, int MatrixOptions> struct ei_matrix_array<T,Size,MatrixOptions,false>
{
T array[Size];
ei_matrix_array() {}
ei_matrix_array(ei_constructor_without_unaligned_array_assert) {}
};
/** \internal
*
* \class ei_matrix_storage
*
* \brief Stores the data of a matrix
*
* This class stores the data of fixed-size, dynamic-size or mixed matrices
* in a way as compact as possible.
*
* \sa Matrix
*/
template<typename T, int Size, int _Rows, int _Cols, int _Options> class ei_matrix_storage;
// purely fixed-size matrix
template<typename T, int Size, int _Rows, int _Cols, int _Options> class ei_matrix_storage
{
ei_matrix_array<T,Size,_Options> m_data;
public:
inline explicit ei_matrix_storage() {}
inline ei_matrix_storage(ei_constructor_without_unaligned_array_assert)
: m_data(ei_constructor_without_unaligned_array_assert()) {}
inline ei_matrix_storage(int,int,int) {}
inline void swap(ei_matrix_storage& other) { std::swap(m_data,other.m_data); }
inline static int rows(void) {return _Rows;}
inline static int cols(void) {return _Cols;}
inline void resize(int,int,int) {}
inline const T *data() const { return m_data.array; }
inline T *data() { return m_data.array; }
};
// dynamic-size matrix with fixed-size storage
template<typename T, int Size, int _Options> class ei_matrix_storage<T, Size, Dynamic, Dynamic, _Options>
{
ei_matrix_array<T,Size,_Options> m_data;
int m_rows;
int m_cols;
public:
inline explicit ei_matrix_storage() : m_rows(0), m_cols(0) {}
inline ei_matrix_storage(ei_constructor_without_unaligned_array_assert)
: m_data(ei_constructor_without_unaligned_array_assert()), m_rows(0), m_cols(0) {}
inline ei_matrix_storage(int, int rows, int cols) : m_rows(rows), m_cols(cols) {}
inline ~ei_matrix_storage() {}
inline void swap(ei_matrix_storage& other)
{ std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); std::swap(m_cols,other.m_cols); }
inline int rows(void) const {return m_rows;}
inline int cols(void) const {return m_cols;}
inline void resize(int, int rows, int cols)
{
m_rows = rows;
m_cols = cols;
}
inline const T *data() const { return m_data.array; }
inline T *data() { return m_data.array; }
};
// dynamic-size matrix with fixed-size storage and fixed width
template<typename T, int Size, int _Cols, int _Options> class ei_matrix_storage<T, Size, Dynamic, _Cols, _Options>
{
ei_matrix_array<T,Size,_Options> m_data;
int m_rows;
public:
inline explicit ei_matrix_storage() : m_rows(0) {}
inline ei_matrix_storage(ei_constructor_without_unaligned_array_assert)
: m_data(ei_constructor_without_unaligned_array_assert()), m_rows(0) {}
inline ei_matrix_storage(int, int rows, int) : m_rows(rows) {}
inline ~ei_matrix_storage() {}
inline void swap(ei_matrix_storage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); }
inline int rows(void) const {return m_rows;}
inline int cols(void) const {return _Cols;}
inline void resize(int /*size*/, int rows, int)
{
m_rows = rows;
}
inline const T *data() const { return m_data.array; }
inline T *data() { return m_data.array; }
};
// dynamic-size matrix with fixed-size storage and fixed height
template<typename T, int Size, int _Rows, int _Options> class ei_matrix_storage<T, Size, _Rows, Dynamic, _Options>
{
ei_matrix_array<T,Size,_Options> m_data;
int m_cols;
public:
inline explicit ei_matrix_storage() : m_cols(0) {}
inline ei_matrix_storage(ei_constructor_without_unaligned_array_assert)
: m_data(ei_constructor_without_unaligned_array_assert()), m_cols(0) {}
inline ei_matrix_storage(int, int, int cols) : m_cols(cols) {}
inline ~ei_matrix_storage() {}
inline void swap(ei_matrix_storage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); }
inline int rows(void) const {return _Rows;}
inline int cols(void) const {return m_cols;}
inline void resize(int, int, int cols)
{
m_cols = cols;
}
inline const T *data() const { return m_data.array; }
inline T *data() { return m_data.array; }
};
// purely dynamic matrix.
template<typename T, int _Options> class ei_matrix_storage<T, Dynamic, Dynamic, Dynamic, _Options>
{
T *m_data;
int m_rows;
int m_cols;
public:
inline explicit ei_matrix_storage() : m_data(0), m_rows(0), m_cols(0) {}
inline ei_matrix_storage(ei_constructor_without_unaligned_array_assert)
: m_data(0), m_rows(0), m_cols(0) {}
inline ei_matrix_storage(int size, int rows, int cols)
: m_data(ei_aligned_new<T>(size)), m_rows(rows), m_cols(cols) {}
inline ~ei_matrix_storage() { ei_aligned_delete(m_data, m_rows*m_cols); }
inline void swap(ei_matrix_storage& other)
{ std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); std::swap(m_cols,other.m_cols); }
inline int rows(void) const {return m_rows;}
inline int cols(void) const {return m_cols;}
void resize(int size, int rows, int cols)
{
if(size != m_rows*m_cols)
{
ei_aligned_delete(m_data, m_rows*m_cols);
if (size)
m_data = ei_aligned_new<T>(size);
else
m_data = 0;
}
m_rows = rows;
m_cols = cols;
}
inline const T *data() const { return m_data; }
inline T *data() { return m_data; }
};
// matrix with dynamic width and fixed height (so that matrix has dynamic size).
template<typename T, int _Rows, int _Options> class ei_matrix_storage<T, Dynamic, _Rows, Dynamic, _Options>
{
T *m_data;
int m_cols;
public:
inline explicit ei_matrix_storage() : m_data(0), m_cols(0) {}
inline ei_matrix_storage(ei_constructor_without_unaligned_array_assert) : m_data(0), m_cols(0) {}
inline ei_matrix_storage(int size, int, int cols) : m_data(ei_aligned_new<T>(size)), m_cols(cols) {}
inline ~ei_matrix_storage() { ei_aligned_delete(m_data, _Rows*m_cols); }
inline void swap(ei_matrix_storage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); }
inline static int rows(void) {return _Rows;}
inline int cols(void) const {return m_cols;}
void resize(int size, int, int cols)
{
if(size != _Rows*m_cols)
{
ei_aligned_delete(m_data, _Rows*m_cols);
if (size)
m_data = ei_aligned_new<T>(size);
else
m_data = 0;
}
m_cols = cols;
}
inline const T *data() const { return m_data; }
inline T *data() { return m_data; }
};
// matrix with dynamic height and fixed width (so that matrix has dynamic size).
template<typename T, int _Cols, int _Options> class ei_matrix_storage<T, Dynamic, Dynamic, _Cols, _Options>
{
T *m_data;
int m_rows;
public:
inline explicit ei_matrix_storage() : m_data(0), m_rows(0) {}
inline ei_matrix_storage(ei_constructor_without_unaligned_array_assert) : m_data(0), m_rows(0) {}
inline ei_matrix_storage(int size, int rows, int) : m_data(ei_aligned_new<T>(size)), m_rows(rows) {}
inline ~ei_matrix_storage() { ei_aligned_delete(m_data, _Cols*m_rows); }
inline void swap(ei_matrix_storage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); }
inline int rows(void) const {return m_rows;}
inline static int cols(void) {return _Cols;}
void resize(int size, int rows, int)
{
if(size != m_rows*_Cols)
{
ei_aligned_delete(m_data, _Cols*m_rows);
if (size)
m_data = ei_aligned_new<T>(size);
else
m_data = 0;
}
m_rows = rows;
}
inline const T *data() const { return m_data; }
inline T *data() { return m_data; }
};
#endif // EIGEN_MATRIX_H

122
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Minor.h Normal file
View File

@ -0,0 +1,122 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_MINOR_H
#define EIGEN_MINOR_H
/** \nonstableyet
* \class Minor
*
* \brief Expression of a minor
*
* \param MatrixType the type of the object in which we are taking a minor
*
* This class represents an expression of a minor. It is the return
* type of MatrixBase::minor() and most of the time this is the only way it
* is used.
*
* \sa MatrixBase::minor()
*/
template<typename MatrixType>
struct ei_traits<Minor<MatrixType> >
{
typedef typename MatrixType::Scalar Scalar;
typedef typename ei_nested<MatrixType>::type MatrixTypeNested;
typedef typename ei_unref<MatrixTypeNested>::type _MatrixTypeNested;
enum {
RowsAtCompileTime = (MatrixType::RowsAtCompileTime != Dynamic) ?
int(MatrixType::RowsAtCompileTime) - 1 : Dynamic,
ColsAtCompileTime = (MatrixType::ColsAtCompileTime != Dynamic) ?
int(MatrixType::ColsAtCompileTime) - 1 : Dynamic,
MaxRowsAtCompileTime = (MatrixType::MaxRowsAtCompileTime != Dynamic) ?
int(MatrixType::MaxRowsAtCompileTime) - 1 : Dynamic,
MaxColsAtCompileTime = (MatrixType::MaxColsAtCompileTime != Dynamic) ?
int(MatrixType::MaxColsAtCompileTime) - 1 : Dynamic,
Flags = _MatrixTypeNested::Flags & HereditaryBits,
CoeffReadCost = _MatrixTypeNested::CoeffReadCost
};
};
template<typename MatrixType> class Minor
: public MatrixBase<Minor<MatrixType> >
{
public:
EIGEN_GENERIC_PUBLIC_INTERFACE(Minor)
inline Minor(const MatrixType& matrix,
int row, int col)
: m_matrix(matrix), m_row(row), m_col(col)
{
ei_assert(row >= 0 && row < matrix.rows()
&& col >= 0 && col < matrix.cols());
}
EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Minor)
inline int rows() const { return m_matrix.rows() - 1; }
inline int cols() const { return m_matrix.cols() - 1; }
inline Scalar& coeffRef(int row, int col)
{
return m_matrix.const_cast_derived().coeffRef(row + (row >= m_row), col + (col >= m_col));
}
inline const Scalar coeff(int row, int col) const
{
return m_matrix.coeff(row + (row >= m_row), col + (col >= m_col));
}
protected:
const typename MatrixType::Nested m_matrix;
const int m_row, m_col;
};
/** \nonstableyet
* \return an expression of the (\a row, \a col)-minor of *this,
* i.e. an expression constructed from *this by removing the specified
* row and column.
*
* Example: \include MatrixBase_minor.cpp
* Output: \verbinclude MatrixBase_minor.out
*
* \sa class Minor
*/
template<typename Derived>
inline Minor<Derived>
MatrixBase<Derived>::minor(int row, int col)
{
return Minor<Derived>(derived(), row, col);
}
/** \nonstableyet
* This is the const version of minor(). */
template<typename Derived>
inline const Minor<Derived>
MatrixBase<Derived>::minor(int row, int col) const
{
return Minor<Derived>(derived(), row, col);
}
#endif // EIGEN_MINOR_H

117
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/NestByValue.h Normal file
View File

@ -0,0 +1,117 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_NESTBYVALUE_H
#define EIGEN_NESTBYVALUE_H
/** \class NestByValue
*
* \brief Expression which must be nested by value
*
* \param ExpressionType the type of the object of which we are requiring nesting-by-value
*
* This class is the return type of MatrixBase::nestByValue()
* and most of the time this is the only way it is used.
*
* \sa MatrixBase::nestByValue()
*/
template<typename ExpressionType>
struct ei_traits<NestByValue<ExpressionType> > : public ei_traits<ExpressionType>
{};
template<typename ExpressionType> class NestByValue
: public MatrixBase<NestByValue<ExpressionType> >
{
public:
EIGEN_GENERIC_PUBLIC_INTERFACE(NestByValue)
inline NestByValue(const ExpressionType& matrix) : m_expression(matrix) {}
inline int rows() const { return m_expression.rows(); }
inline int cols() const { return m_expression.cols(); }
inline int stride() const { return m_expression.stride(); }
inline const Scalar coeff(int row, int col) const
{
return m_expression.coeff(row, col);
}
inline Scalar& coeffRef(int row, int col)
{
return m_expression.const_cast_derived().coeffRef(row, col);
}
inline const Scalar coeff(int index) const
{
return m_expression.coeff(index);
}
inline Scalar& coeffRef(int index)
{
return m_expression.const_cast_derived().coeffRef(index);
}
template<int LoadMode>
inline const PacketScalar packet(int row, int col) const
{
return m_expression.template packet<LoadMode>(row, col);
}
template<int LoadMode>
inline void writePacket(int row, int col, const PacketScalar& x)
{
m_expression.const_cast_derived().template writePacket<LoadMode>(row, col, x);
}
template<int LoadMode>
inline const PacketScalar packet(int index) const
{
return m_expression.template packet<LoadMode>(index);
}
template<int LoadMode>
inline void writePacket(int index, const PacketScalar& x)
{
m_expression.const_cast_derived().template writePacket<LoadMode>(index, x);
}
protected:
const ExpressionType m_expression;
private:
NestByValue& operator=(const NestByValue&);
};
/** \returns an expression of the temporary version of *this.
*/
template<typename Derived>
inline const NestByValue<Derived>
MatrixBase<Derived>::nestByValue() const
{
return NestByValue<Derived>(derived());
}
#endif // EIGEN_NESTBYVALUE_H

142
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/NumTraits.h Normal file
View File

@ -0,0 +1,142 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_NUMTRAITS_H
#define EIGEN_NUMTRAITS_H
/** \class NumTraits
*
* \brief Holds some data about the various numeric (i.e. scalar) types allowed by Eigen.
*
* \param T the numeric type about which this class provides data. Recall that Eigen allows
* only the following types for \a T: \c int, \c float, \c double,
* \c std::complex<float>, \c std::complex<double>, and \c long \c double (especially
* useful to enforce x87 arithmetics when SSE is the default).
*
* The provided data consists of:
* \li A typedef \a Real, giving the "real part" type of \a T. If \a T is already real,
* then \a Real is just a typedef to \a T. If \a T is \c std::complex<U> then \a Real
* is a typedef to \a U.
* \li A typedef \a FloatingPoint, giving the "floating-point type" of \a T. If \a T is
* \c int, then \a FloatingPoint is a typedef to \c double. Otherwise, \a FloatingPoint
* is a typedef to \a T.
* \li An enum value \a IsComplex. It is equal to 1 if \a T is a \c std::complex
* type, and to 0 otherwise.
* \li An enum \a HasFloatingPoint. It is equal to \c 0 if \a T is \c int,
* and to \c 1 otherwise.
*/
template<typename T> struct NumTraits;
template<> struct NumTraits<int>
{
typedef int Real;
typedef double FloatingPoint;
enum {
IsComplex = 0,
HasFloatingPoint = 0,
ReadCost = 1,
AddCost = 1,
MulCost = 1
};
};
template<> struct NumTraits<float>
{
typedef float Real;
typedef float FloatingPoint;
enum {
IsComplex = 0,
HasFloatingPoint = 1,
ReadCost = 1,
AddCost = 1,
MulCost = 1
};
};
template<> struct NumTraits<double>
{
typedef double Real;
typedef double FloatingPoint;
enum {
IsComplex = 0,
HasFloatingPoint = 1,
ReadCost = 1,
AddCost = 1,
MulCost = 1
};
};
template<typename _Real> struct NumTraits<std::complex<_Real> >
{
typedef _Real Real;
typedef std::complex<_Real> FloatingPoint;
enum {
IsComplex = 1,
HasFloatingPoint = NumTraits<Real>::HasFloatingPoint,
ReadCost = 2 * NumTraits<_Real>::ReadCost,
AddCost = 2 * NumTraits<Real>::AddCost,
MulCost = 4 * NumTraits<Real>::MulCost + 2 * NumTraits<Real>::AddCost
};
};
template<> struct NumTraits<long long int>
{
typedef long long int Real;
typedef long double FloatingPoint;
enum {
IsComplex = 0,
HasFloatingPoint = 0,
ReadCost = 1,
AddCost = 1,
MulCost = 1
};
};
template<> struct NumTraits<long double>
{
typedef long double Real;
typedef long double FloatingPoint;
enum {
IsComplex = 0,
HasFloatingPoint = 1,
ReadCost = 1,
AddCost = 1,
MulCost = 1
};
};
template<> struct NumTraits<bool>
{
typedef bool Real;
typedef float FloatingPoint;
enum {
IsComplex = 0,
HasFloatingPoint = 0,
ReadCost = 1,
AddCost = 1,
MulCost = 1
};
};
#endif // EIGEN_NUMTRAITS_H

377
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Part.h Normal file
View File

@ -0,0 +1,377 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_PART_H
#define EIGEN_PART_H
/** \nonstableyet
* \class Part
*
* \brief Expression of a triangular matrix extracted from a given matrix
*
* \param MatrixType the type of the object in which we are taking the triangular part
* \param Mode the kind of triangular matrix expression to construct. Can be UpperTriangular, StrictlyUpperTriangular,
* UnitUpperTriangular, LowerTriangular, StrictlyLowerTriangular, UnitLowerTriangular. This is in fact a bit field; it must have either
* UpperTriangularBit or LowerTriangularBit, and additionnaly it may have either ZeroDiagBit or
* UnitDiagBit.
*
* This class represents an expression of the upper or lower triangular part of
* a square matrix, possibly with a further assumption on the diagonal. It is the return type
* of MatrixBase::part() and most of the time this is the only way it is used.
*
* \sa MatrixBase::part()
*/
template<typename MatrixType, unsigned int Mode>
struct ei_traits<Part<MatrixType, Mode> > : ei_traits<MatrixType>
{
typedef typename ei_nested<MatrixType>::type MatrixTypeNested;
typedef typename ei_unref<MatrixTypeNested>::type _MatrixTypeNested;
enum {
Flags = (_MatrixTypeNested::Flags & (HereditaryBits) & (~(PacketAccessBit | DirectAccessBit | LinearAccessBit))) | Mode,
CoeffReadCost = _MatrixTypeNested::CoeffReadCost + ConditionalJumpCost
};
};
template<typename MatrixType, unsigned int Mode> class Part
: public MatrixBase<Part<MatrixType, Mode> >
{
public:
EIGEN_GENERIC_PUBLIC_INTERFACE(Part)
inline Part(const MatrixType& matrix) : m_matrix(matrix)
{ ei_assert(ei_are_flags_consistent<Mode>::ret); }
/** \sa MatrixBase::operator+=() */
template<typename Other> Part& operator+=(const Other& other);
/** \sa MatrixBase::operator-=() */
template<typename Other> Part& operator-=(const Other& other);
/** \sa MatrixBase::operator*=() */
Part& operator*=(const typename ei_traits<MatrixType>::Scalar& other);
/** \sa MatrixBase::operator/=() */
Part& operator/=(const typename ei_traits<MatrixType>::Scalar& other);
/** \sa operator=(), MatrixBase::lazyAssign() */
template<typename Other> void lazyAssign(const Other& other);
/** \sa MatrixBase::operator=() */
template<typename Other> Part& operator=(const Other& other);
inline int rows() const { return m_matrix.rows(); }
inline int cols() const { return m_matrix.cols(); }
inline int stride() const { return m_matrix.stride(); }
inline Scalar coeff(int row, int col) const
{
// SelfAdjointBit doesn't play any role here: just because a matrix is selfadjoint doesn't say anything about
// each individual coefficient, except for the not-very-useful-here fact that diagonal coefficients are real.
if( ((Flags & LowerTriangularBit) && (col>row)) || ((Flags & UpperTriangularBit) && (row>col)) )
return (Scalar)0;
if(Flags & UnitDiagBit)
return col==row ? (Scalar)1 : m_matrix.coeff(row, col);
else if(Flags & ZeroDiagBit)
return col==row ? (Scalar)0 : m_matrix.coeff(row, col);
else
return m_matrix.coeff(row, col);
}
inline Scalar& coeffRef(int row, int col)
{
EIGEN_STATIC_ASSERT(!(Flags & UnitDiagBit), WRITING_TO_TRIANGULAR_PART_WITH_UNIT_DIAGONAL_IS_NOT_SUPPORTED)
EIGEN_STATIC_ASSERT(!(Flags & SelfAdjointBit), COEFFICIENT_WRITE_ACCESS_TO_SELFADJOINT_NOT_SUPPORTED)
ei_assert( (Mode==UpperTriangular && col>=row)
|| (Mode==LowerTriangular && col<=row)
|| (Mode==StrictlyUpperTriangular && col>row)
|| (Mode==StrictlyLowerTriangular && col<row));
return m_matrix.const_cast_derived().coeffRef(row, col);
}
/** \internal */
const MatrixType& _expression() const { return m_matrix; }
/** discard any writes to a row */
const Block<Part, 1, ColsAtCompileTime> row(int i) { return Base::row(i); }
const Block<Part, 1, ColsAtCompileTime> row(int i) const { return Base::row(i); }
/** discard any writes to a column */
const Block<Part, RowsAtCompileTime, 1> col(int i) { return Base::col(i); }
const Block<Part, RowsAtCompileTime, 1> col(int i) const { return Base::col(i); }
template<typename OtherDerived>
void swap(const MatrixBase<OtherDerived>& other)
{
Part<SwapWrapper<MatrixType>,Mode>(const_cast<MatrixType&>(m_matrix)).lazyAssign(other.derived());
}
protected:
const typename MatrixType::Nested m_matrix;
private:
Part& operator=(const Part&);
};
/** \nonstableyet
* \returns an expression of a triangular matrix extracted from the current matrix
*
* The parameter \a Mode can have the following values: \c UpperTriangular, \c StrictlyUpperTriangular, \c UnitUpperTriangular,
* \c LowerTriangular, \c StrictlyLowerTriangular, \c UnitLowerTriangular.
*
* \addexample PartExample \label How to extract a triangular part of an arbitrary matrix
*
* Example: \include MatrixBase_extract.cpp
* Output: \verbinclude MatrixBase_extract.out
*
* \sa class Part, part(), marked()
*/
template<typename Derived>
template<unsigned int Mode>
const Part<Derived, Mode> MatrixBase<Derived>::part() const
{
return derived();
}
template<typename MatrixType, unsigned int Mode>
template<typename Other>
inline Part<MatrixType, Mode>& Part<MatrixType, Mode>::operator=(const Other& other)
{
if(Other::Flags & EvalBeforeAssigningBit)
{
typename MatrixBase<Other>::PlainMatrixType other_evaluated(other.rows(), other.cols());
other_evaluated.template part<Mode>().lazyAssign(other);
lazyAssign(other_evaluated);
}
else
lazyAssign(other.derived());
return *this;
}
template<typename Derived1, typename Derived2, unsigned int Mode, int UnrollCount>
struct ei_part_assignment_impl
{
enum {
col = (UnrollCount-1) / Derived1::RowsAtCompileTime,
row = (UnrollCount-1) % Derived1::RowsAtCompileTime
};
inline static void run(Derived1 &dst, const Derived2 &src)
{
ei_part_assignment_impl<Derived1, Derived2, Mode, UnrollCount-1>::run(dst, src);
if(Mode == SelfAdjoint)
{
if(row == col)
dst.coeffRef(row, col) = ei_real(src.coeff(row, col));
else if(row < col)
dst.coeffRef(col, row) = ei_conj(dst.coeffRef(row, col) = src.coeff(row, col));
}
else
{
ei_assert(Mode == UpperTriangular || Mode == LowerTriangular || Mode == StrictlyUpperTriangular || Mode == StrictlyLowerTriangular);
if((Mode == UpperTriangular && row <= col)
|| (Mode == LowerTriangular && row >= col)
|| (Mode == StrictlyUpperTriangular && row < col)
|| (Mode == StrictlyLowerTriangular && row > col))
dst.copyCoeff(row, col, src);
}
}
};
template<typename Derived1, typename Derived2, unsigned int Mode>
struct ei_part_assignment_impl<Derived1, Derived2, Mode, 1>
{
inline static void run(Derived1 &dst, const Derived2 &src)
{
if(!(Mode & ZeroDiagBit))
dst.copyCoeff(0, 0, src);
}
};
// prevent buggy user code from causing an infinite recursion
template<typename Derived1, typename Derived2, unsigned int Mode>
struct ei_part_assignment_impl<Derived1, Derived2, Mode, 0>
{
inline static void run(Derived1 &, const Derived2 &) {}
};
template<typename Derived1, typename Derived2>
struct ei_part_assignment_impl<Derived1, Derived2, UpperTriangular, Dynamic>
{
inline static void run(Derived1 &dst, const Derived2 &src)
{
for(int j = 0; j < dst.cols(); ++j)
for(int i = 0; i <= j; ++i)
dst.copyCoeff(i, j, src);
}
};
template<typename Derived1, typename Derived2>
struct ei_part_assignment_impl<Derived1, Derived2, LowerTriangular, Dynamic>
{
inline static void run(Derived1 &dst, const Derived2 &src)
{
for(int j = 0; j < dst.cols(); ++j)
for(int i = j; i < dst.rows(); ++i)
dst.copyCoeff(i, j, src);
}
};
template<typename Derived1, typename Derived2>
struct ei_part_assignment_impl<Derived1, Derived2, StrictlyUpperTriangular, Dynamic>
{
inline static void run(Derived1 &dst, const Derived2 &src)
{
for(int j = 0; j < dst.cols(); ++j)
for(int i = 0; i < j; ++i)
dst.copyCoeff(i, j, src);
}
};
template<typename Derived1, typename Derived2>
struct ei_part_assignment_impl<Derived1, Derived2, StrictlyLowerTriangular, Dynamic>
{
inline static void run(Derived1 &dst, const Derived2 &src)
{
for(int j = 0; j < dst.cols(); ++j)
for(int i = j+1; i < dst.rows(); ++i)
dst.copyCoeff(i, j, src);
}
};
template<typename Derived1, typename Derived2>
struct ei_part_assignment_impl<Derived1, Derived2, SelfAdjoint, Dynamic>
{
inline static void run(Derived1 &dst, const Derived2 &src)
{
for(int j = 0; j < dst.cols(); ++j)
{
for(int i = 0; i < j; ++i)
dst.coeffRef(j, i) = ei_conj(dst.coeffRef(i, j) = src.coeff(i, j));
dst.coeffRef(j, j) = ei_real(src.coeff(j, j));
}
}
};
template<typename MatrixType, unsigned int Mode>
template<typename Other>
void Part<MatrixType, Mode>::lazyAssign(const Other& other)
{
const bool unroll = MatrixType::SizeAtCompileTime * Other::CoeffReadCost / 2 <= EIGEN_UNROLLING_LIMIT;
ei_assert(m_matrix.rows() == other.rows() && m_matrix.cols() == other.cols());
ei_part_assignment_impl
<MatrixType, Other, Mode,
unroll ? int(MatrixType::SizeAtCompileTime) : Dynamic
>::run(m_matrix.const_cast_derived(), other.derived());
}
/** \nonstableyet
* \returns a lvalue pseudo-expression allowing to perform special operations on \c *this.
*
* The \a Mode parameter can have the following values: \c UpperTriangular, \c StrictlyUpperTriangular, \c LowerTriangular,
* \c StrictlyLowerTriangular, \c SelfAdjoint.
*
* \addexample PartExample \label How to write to a triangular part of a matrix
*
* Example: \include MatrixBase_part.cpp
* Output: \verbinclude MatrixBase_part.out
*
* \sa class Part, MatrixBase::extract(), MatrixBase::marked()
*/
template<typename Derived>
template<unsigned int Mode>
inline Part<Derived, Mode> MatrixBase<Derived>::part()
{
return Part<Derived, Mode>(derived());
}
/** \returns true if *this is approximately equal to an upper triangular matrix,
* within the precision given by \a prec.
*
* \sa isLowerTriangular(), extract(), part(), marked()
*/
template<typename Derived>
bool MatrixBase<Derived>::isUpperTriangular(RealScalar prec) const
{
if(cols() != rows()) return false;
RealScalar maxAbsOnUpperTriangularPart = static_cast<RealScalar>(-1);
for(int j = 0; j < cols(); ++j)
for(int i = 0; i <= j; ++i)
{
RealScalar absValue = ei_abs(coeff(i,j));
if(absValue > maxAbsOnUpperTriangularPart) maxAbsOnUpperTriangularPart = absValue;
}
for(int j = 0; j < cols()-1; ++j)
for(int i = j+1; i < rows(); ++i)
if(!ei_isMuchSmallerThan(coeff(i, j), maxAbsOnUpperTriangularPart, prec)) return false;
return true;
}
/** \returns true if *this is approximately equal to a lower triangular matrix,
* within the precision given by \a prec.
*
* \sa isUpperTriangular(), extract(), part(), marked()
*/
template<typename Derived>
bool MatrixBase<Derived>::isLowerTriangular(RealScalar prec) const
{
if(cols() != rows()) return false;
RealScalar maxAbsOnLowerTriangularPart = static_cast<RealScalar>(-1);
for(int j = 0; j < cols(); ++j)
for(int i = j; i < rows(); ++i)
{
RealScalar absValue = ei_abs(coeff(i,j));
if(absValue > maxAbsOnLowerTriangularPart) maxAbsOnLowerTriangularPart = absValue;
}
for(int j = 1; j < cols(); ++j)
for(int i = 0; i < j; ++i)
if(!ei_isMuchSmallerThan(coeff(i, j), maxAbsOnLowerTriangularPart, prec)) return false;
return true;
}
template<typename MatrixType, unsigned int Mode>
template<typename Other>
inline Part<MatrixType, Mode>& Part<MatrixType, Mode>::operator+=(const Other& other)
{
return *this = m_matrix + other;
}
template<typename MatrixType, unsigned int Mode>
template<typename Other>
inline Part<MatrixType, Mode>& Part<MatrixType, Mode>::operator-=(const Other& other)
{
return *this = m_matrix - other;
}
template<typename MatrixType, unsigned int Mode>
inline Part<MatrixType, Mode>& Part<MatrixType, Mode>::operator*=
(const typename ei_traits<MatrixType>::Scalar& other)
{
return *this = m_matrix * other;
}
template<typename MatrixType, unsigned int Mode>
inline Part<MatrixType, Mode>& Part<MatrixType, Mode>::operator/=
(const typename ei_traits<MatrixType>::Scalar& other)
{
return *this = m_matrix / other;
}
#endif // EIGEN_PART_H

807
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Product.h Normal file
View File

@ -0,0 +1,807 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_PRODUCT_H
#define EIGEN_PRODUCT_H
/***************************
*** Forward declarations ***
***************************/
template<int VectorizationMode, int Index, typename Lhs, typename Rhs, typename RetScalar>
struct ei_product_coeff_impl;
template<int StorageOrder, int Index, typename Lhs, typename Rhs, typename PacketScalar, int LoadMode>
struct ei_product_packet_impl;
/** \class ProductReturnType
*
* \brief Helper class to get the correct and optimized returned type of operator*
*
* \param Lhs the type of the left-hand side
* \param Rhs the type of the right-hand side
* \param ProductMode the type of the product (determined automatically by ei_product_mode)
*
* This class defines the typename Type representing the optimized product expression
* between two matrix expressions. In practice, using ProductReturnType<Lhs,Rhs>::Type
* is the recommended way to define the result type of a function returning an expression
* which involve a matrix product. The class Product or DiagonalProduct should never be
* used directly.
*
* \sa class Product, class DiagonalProduct, MatrixBase::operator*(const MatrixBase<OtherDerived>&)
*/
template<typename Lhs, typename Rhs, int ProductMode>
struct ProductReturnType
{
typedef typename ei_nested<Lhs,Rhs::ColsAtCompileTime>::type LhsNested;
typedef typename ei_nested<Rhs,Lhs::RowsAtCompileTime>::type RhsNested;
typedef Product<LhsNested, RhsNested, ProductMode> Type;
};
// cache friendly specialization
// note that there is a DiagonalProduct specialization in DiagonalProduct.h
template<typename Lhs, typename Rhs>
struct ProductReturnType<Lhs,Rhs,CacheFriendlyProduct>
{
typedef const Lhs& LhsNested;
typedef const Rhs& RhsNested;
typedef Product<LhsNested, RhsNested, CacheFriendlyProduct> Type;
};
/* Helper class to determine the type of the product, can be either:
* - NormalProduct
* - CacheFriendlyProduct
* - DiagonalProduct
*/
template<typename Lhs, typename Rhs> struct ei_product_mode
{
enum{
value = ((Rhs::Flags&Diagonal)==Diagonal) || ((Lhs::Flags&Diagonal)==Diagonal)
? DiagonalProduct
: Lhs::MaxColsAtCompileTime == Dynamic
&& ( Lhs::MaxRowsAtCompileTime == Dynamic
|| Rhs::MaxColsAtCompileTime == Dynamic )
&& (!(Rhs::IsVectorAtCompileTime && (Lhs::Flags&RowMajorBit) && (!(Lhs::Flags&DirectAccessBit))))
&& (!(Lhs::IsVectorAtCompileTime && (!(Rhs::Flags&RowMajorBit)) && (!(Rhs::Flags&DirectAccessBit))))
&& (ei_is_same_type<typename Lhs::Scalar, typename Rhs::Scalar>::ret)
? CacheFriendlyProduct
: NormalProduct };
};
/** \class Product
*
* \brief Expression of the product of two matrices
*
* \param LhsNested the type used to store the left-hand side
* \param RhsNested the type used to store the right-hand side
* \param ProductMode the type of the product
*
* This class represents an expression of the product of two matrices.
* It is the return type of the operator* between matrices. Its template
* arguments are determined automatically by ProductReturnType. Therefore,
* Product should never be used direclty. To determine the result type of a
* function which involves a matrix product, use ProductReturnType::Type.
*
* \sa ProductReturnType, MatrixBase::operator*(const MatrixBase<OtherDerived>&)
*/
template<typename LhsNested, typename RhsNested, int ProductMode>
struct ei_traits<Product<LhsNested, RhsNested, ProductMode> >
{
// clean the nested types:
typedef typename ei_cleantype<LhsNested>::type _LhsNested;
typedef typename ei_cleantype<RhsNested>::type _RhsNested;
typedef typename ei_scalar_product_traits<typename _LhsNested::Scalar, typename _RhsNested::Scalar>::ReturnType Scalar;
enum {
LhsCoeffReadCost = _LhsNested::CoeffReadCost,
RhsCoeffReadCost = _RhsNested::CoeffReadCost,
LhsFlags = _LhsNested::Flags,
RhsFlags = _RhsNested::Flags,
RowsAtCompileTime = _LhsNested::RowsAtCompileTime,
ColsAtCompileTime = _RhsNested::ColsAtCompileTime,
InnerSize = EIGEN_SIZE_MIN(_LhsNested::ColsAtCompileTime, _RhsNested::RowsAtCompileTime),
MaxRowsAtCompileTime = _LhsNested::MaxRowsAtCompileTime,
MaxColsAtCompileTime = _RhsNested::MaxColsAtCompileTime,
LhsRowMajor = LhsFlags & RowMajorBit,
RhsRowMajor = RhsFlags & RowMajorBit,
CanVectorizeRhs = RhsRowMajor && (RhsFlags & PacketAccessBit)
&& (ColsAtCompileTime % ei_packet_traits<Scalar>::size == 0),
CanVectorizeLhs = (!LhsRowMajor) && (LhsFlags & PacketAccessBit)
&& (RowsAtCompileTime % ei_packet_traits<Scalar>::size == 0),
EvalToRowMajor = RhsRowMajor && (ProductMode==(int)CacheFriendlyProduct ? LhsRowMajor : (!CanVectorizeLhs)),
RemovedBits = ~((EvalToRowMajor ? 0 : RowMajorBit)|DirectAccessBit),
Flags = ((unsigned int)(LhsFlags | RhsFlags) & HereditaryBits & RemovedBits)
| EvalBeforeAssigningBit
| EvalBeforeNestingBit
| (CanVectorizeLhs || CanVectorizeRhs ? PacketAccessBit : 0)
| (LhsFlags & RhsFlags & AlignedBit),
CoeffReadCost = InnerSize == Dynamic ? Dynamic
: InnerSize * (NumTraits<Scalar>::MulCost + LhsCoeffReadCost + RhsCoeffReadCost)
+ (InnerSize - 1) * NumTraits<Scalar>::AddCost,
/* CanVectorizeInner deserves special explanation. It does not affect the product flags. It is not used outside
* of Product. If the Product itself is not a packet-access expression, there is still a chance that the inner
* loop of the product might be vectorized. This is the meaning of CanVectorizeInner. Since it doesn't affect
* the Flags, it is safe to make this value depend on ActualPacketAccessBit, that doesn't affect the ABI.
*/
CanVectorizeInner = LhsRowMajor && (!RhsRowMajor) && (LhsFlags & RhsFlags & ActualPacketAccessBit)
&& (InnerSize % ei_packet_traits<Scalar>::size == 0)
};
};
template<typename LhsNested, typename RhsNested, int ProductMode> class Product : ei_no_assignment_operator,
public MatrixBase<Product<LhsNested, RhsNested, ProductMode> >
{
public:
EIGEN_GENERIC_PUBLIC_INTERFACE(Product)
private:
typedef typename ei_traits<Product>::_LhsNested _LhsNested;
typedef typename ei_traits<Product>::_RhsNested _RhsNested;
enum {
PacketSize = ei_packet_traits<Scalar>::size,
InnerSize = ei_traits<Product>::InnerSize,
Unroll = CoeffReadCost <= EIGEN_UNROLLING_LIMIT,
CanVectorizeInner = ei_traits<Product>::CanVectorizeInner
};
typedef ei_product_coeff_impl<CanVectorizeInner ? InnerVectorization : NoVectorization,
Unroll ? InnerSize-1 : Dynamic,
_LhsNested, _RhsNested, Scalar> ScalarCoeffImpl;
public:
template<typename Lhs, typename Rhs>
inline Product(const Lhs& lhs, const Rhs& rhs)
: m_lhs(lhs), m_rhs(rhs)
{
// we don't allow taking products of matrices of different real types, as that wouldn't be vectorizable.
// We still allow to mix T and complex<T>.
EIGEN_STATIC_ASSERT((ei_is_same_type<typename Lhs::RealScalar, typename Rhs::RealScalar>::ret),
YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
ei_assert(lhs.cols() == rhs.rows()
&& "invalid matrix product"
&& "if you wanted a coeff-wise or a dot product use the respective explicit functions");
}
/** \internal
* compute \a res += \c *this using the cache friendly product.
*/
template<typename DestDerived>
void _cacheFriendlyEvalAndAdd(DestDerived& res) const;
/** \internal
* \returns whether it is worth it to use the cache friendly product.
*/
EIGEN_STRONG_INLINE bool _useCacheFriendlyProduct() const
{
return m_lhs.cols()>=EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD
&& ( rows()>=EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD
|| cols()>=EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD);
}
EIGEN_STRONG_INLINE int rows() const { return m_lhs.rows(); }
EIGEN_STRONG_INLINE int cols() const { return m_rhs.cols(); }
EIGEN_STRONG_INLINE const Scalar coeff(int row, int col) const
{
Scalar res;
ScalarCoeffImpl::run(row, col, m_lhs, m_rhs, res);
return res;
}
/* Allow index-based non-packet access. It is impossible though to allow index-based packed access,
* which is why we don't set the LinearAccessBit.
*/
EIGEN_STRONG_INLINE const Scalar coeff(int index) const
{
Scalar res;
const int row = RowsAtCompileTime == 1 ? 0 : index;
const int col = RowsAtCompileTime == 1 ? index : 0;
ScalarCoeffImpl::run(row, col, m_lhs, m_rhs, res);
return res;
}
template<int LoadMode>
EIGEN_STRONG_INLINE const PacketScalar packet(int row, int col) const
{
PacketScalar res;
ei_product_packet_impl<Flags&RowMajorBit ? RowMajor : ColMajor,
Unroll ? InnerSize-1 : Dynamic,
_LhsNested, _RhsNested, PacketScalar, LoadMode>
::run(row, col, m_lhs, m_rhs, res);
return res;
}
EIGEN_STRONG_INLINE const _LhsNested& lhs() const { return m_lhs; }
EIGEN_STRONG_INLINE const _RhsNested& rhs() const { return m_rhs; }
protected:
const LhsNested m_lhs;
const RhsNested m_rhs;
};
/** \returns the matrix product of \c *this and \a other.
*
* \note If instead of the matrix product you want the coefficient-wise product, see Cwise::operator*().
*
* \sa lazy(), operator*=(const MatrixBase&), Cwise::operator*()
*/
template<typename Derived>
template<typename OtherDerived>
inline const typename ProductReturnType<Derived,OtherDerived>::Type
MatrixBase<Derived>::operator*(const MatrixBase<OtherDerived> &other) const
{
enum {
ProductIsValid = Derived::ColsAtCompileTime==Dynamic
|| OtherDerived::RowsAtCompileTime==Dynamic
|| int(Derived::ColsAtCompileTime)==int(OtherDerived::RowsAtCompileTime),
AreVectors = Derived::IsVectorAtCompileTime && OtherDerived::IsVectorAtCompileTime,
SameSizes = EIGEN_PREDICATE_SAME_MATRIX_SIZE(Derived,OtherDerived)
};
// note to the lost user:
// * for a dot product use: v1.dot(v2)
// * for a coeff-wise product use: v1.cwise()*v2
EIGEN_STATIC_ASSERT(ProductIsValid || !(AreVectors && SameSizes),
INVALID_VECTOR_VECTOR_PRODUCT__IF_YOU_WANTED_A_DOT_OR_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTIONS)
EIGEN_STATIC_ASSERT(ProductIsValid || !(SameSizes && !AreVectors),
INVALID_MATRIX_PRODUCT__IF_YOU_WANTED_A_COEFF_WISE_PRODUCT_YOU_MUST_USE_THE_EXPLICIT_FUNCTION)
EIGEN_STATIC_ASSERT(ProductIsValid || SameSizes, INVALID_MATRIX_PRODUCT)
return typename ProductReturnType<Derived,OtherDerived>::Type(derived(), other.derived());
}
/** replaces \c *this by \c *this * \a other.
*
* \returns a reference to \c *this
*/
template<typename Derived>
template<typename OtherDerived>
inline Derived &
MatrixBase<Derived>::operator*=(const MatrixBase<OtherDerived> &other)
{
return derived() = derived() * other.derived();
}
/***************************************************************************
* Normal product .coeff() implementation (with meta-unrolling)
***************************************************************************/
/**************************************
*** Scalar path - no vectorization ***
**************************************/
template<int Index, typename Lhs, typename Rhs, typename RetScalar>
struct ei_product_coeff_impl<NoVectorization, Index, Lhs, Rhs, RetScalar>
{
EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, RetScalar &res)
{
ei_product_coeff_impl<NoVectorization, Index-1, Lhs, Rhs, RetScalar>::run(row, col, lhs, rhs, res);
res += lhs.coeff(row, Index) * rhs.coeff(Index, col);
}
};
template<typename Lhs, typename Rhs, typename RetScalar>
struct ei_product_coeff_impl<NoVectorization, 0, Lhs, Rhs, RetScalar>
{
EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, RetScalar &res)
{
res = lhs.coeff(row, 0) * rhs.coeff(0, col);
}
};
template<typename Lhs, typename Rhs, typename RetScalar>
struct ei_product_coeff_impl<NoVectorization, Dynamic, Lhs, Rhs, RetScalar>
{
EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, RetScalar& res)
{
ei_assert(lhs.cols()>0 && "you are using a non initialized matrix");
res = lhs.coeff(row, 0) * rhs.coeff(0, col);
for(int i = 1; i < lhs.cols(); ++i)
res += lhs.coeff(row, i) * rhs.coeff(i, col);
}
};
// prevent buggy user code from causing an infinite recursion
template<typename Lhs, typename Rhs, typename RetScalar>
struct ei_product_coeff_impl<NoVectorization, -1, Lhs, Rhs, RetScalar>
{
EIGEN_STRONG_INLINE static void run(int, int, const Lhs&, const Rhs&, RetScalar&) {}
};
/*******************************************
*** Scalar path with inner vectorization ***
*******************************************/
template<int Index, typename Lhs, typename Rhs, typename PacketScalar>
struct ei_product_coeff_vectorized_unroller
{
enum { PacketSize = ei_packet_traits<typename Lhs::Scalar>::size };
EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, typename Lhs::PacketScalar &pres)
{
ei_product_coeff_vectorized_unroller<Index-PacketSize, Lhs, Rhs, PacketScalar>::run(row, col, lhs, rhs, pres);
pres = ei_padd(pres, ei_pmul( lhs.template packet<Aligned>(row, Index) , rhs.template packet<Aligned>(Index, col) ));
}
};
template<typename Lhs, typename Rhs, typename PacketScalar>
struct ei_product_coeff_vectorized_unroller<0, Lhs, Rhs, PacketScalar>
{
EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, typename Lhs::PacketScalar &pres)
{
pres = ei_pmul(lhs.template packet<Aligned>(row, 0) , rhs.template packet<Aligned>(0, col));
}
};
template<int Index, typename Lhs, typename Rhs, typename RetScalar>
struct ei_product_coeff_impl<InnerVectorization, Index, Lhs, Rhs, RetScalar>
{
typedef typename Lhs::PacketScalar PacketScalar;
enum { PacketSize = ei_packet_traits<typename Lhs::Scalar>::size };
EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, RetScalar &res)
{
PacketScalar pres;
ei_product_coeff_vectorized_unroller<Index+1-PacketSize, Lhs, Rhs, PacketScalar>::run(row, col, lhs, rhs, pres);
ei_product_coeff_impl<NoVectorization,Index,Lhs,Rhs,RetScalar>::run(row, col, lhs, rhs, res);
res = ei_predux(pres);
}
};
template<typename Lhs, typename Rhs, int LhsRows = Lhs::RowsAtCompileTime, int RhsCols = Rhs::ColsAtCompileTime>
struct ei_product_coeff_vectorized_dyn_selector
{
EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, typename Lhs::Scalar &res)
{
res = ei_dot_impl<
Block<Lhs, 1, ei_traits<Lhs>::ColsAtCompileTime>,
Block<Rhs, ei_traits<Rhs>::RowsAtCompileTime, 1>,
LinearVectorization, NoUnrolling>::run(lhs.row(row), rhs.col(col));
}
};
// NOTE the 3 following specializations are because taking .col(0) on a vector is a bit slower
// NOTE maybe they are now useless since we have a specialization for Block<Matrix>
template<typename Lhs, typename Rhs, int RhsCols>
struct ei_product_coeff_vectorized_dyn_selector<Lhs,Rhs,1,RhsCols>
{
EIGEN_STRONG_INLINE static void run(int /*row*/, int col, const Lhs& lhs, const Rhs& rhs, typename Lhs::Scalar &res)
{
res = ei_dot_impl<
Lhs,
Block<Rhs, ei_traits<Rhs>::RowsAtCompileTime, 1>,
LinearVectorization, NoUnrolling>::run(lhs, rhs.col(col));
}
};
template<typename Lhs, typename Rhs, int LhsRows>
struct ei_product_coeff_vectorized_dyn_selector<Lhs,Rhs,LhsRows,1>
{
EIGEN_STRONG_INLINE static void run(int row, int /*col*/, const Lhs& lhs, const Rhs& rhs, typename Lhs::Scalar &res)
{
res = ei_dot_impl<
Block<Lhs, 1, ei_traits<Lhs>::ColsAtCompileTime>,
Rhs,
LinearVectorization, NoUnrolling>::run(lhs.row(row), rhs);
}
};
template<typename Lhs, typename Rhs>
struct ei_product_coeff_vectorized_dyn_selector<Lhs,Rhs,1,1>
{
EIGEN_STRONG_INLINE static void run(int /*row*/, int /*col*/, const Lhs& lhs, const Rhs& rhs, typename Lhs::Scalar &res)
{
res = ei_dot_impl<
Lhs,
Rhs,
LinearVectorization, NoUnrolling>::run(lhs, rhs);
}
};
template<typename Lhs, typename Rhs, typename RetScalar>
struct ei_product_coeff_impl<InnerVectorization, Dynamic, Lhs, Rhs, RetScalar>
{
EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, typename Lhs::Scalar &res)
{
ei_product_coeff_vectorized_dyn_selector<Lhs,Rhs>::run(row, col, lhs, rhs, res);
}
};
/*******************
*** Packet path ***
*******************/
template<int Index, typename Lhs, typename Rhs, typename PacketScalar, int LoadMode>
struct ei_product_packet_impl<RowMajor, Index, Lhs, Rhs, PacketScalar, LoadMode>
{
EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, PacketScalar &res)
{
ei_product_packet_impl<RowMajor, Index-1, Lhs, Rhs, PacketScalar, LoadMode>::run(row, col, lhs, rhs, res);
res = ei_pmadd(ei_pset1(lhs.coeff(row, Index)), rhs.template packet<LoadMode>(Index, col), res);
}
};
template<int Index, typename Lhs, typename Rhs, typename PacketScalar, int LoadMode>
struct ei_product_packet_impl<ColMajor, Index, Lhs, Rhs, PacketScalar, LoadMode>
{
EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, PacketScalar &res)
{
ei_product_packet_impl<ColMajor, Index-1, Lhs, Rhs, PacketScalar, LoadMode>::run(row, col, lhs, rhs, res);
res = ei_pmadd(lhs.template packet<LoadMode>(row, Index), ei_pset1(rhs.coeff(Index, col)), res);
}
};
template<typename Lhs, typename Rhs, typename PacketScalar, int LoadMode>
struct ei_product_packet_impl<RowMajor, 0, Lhs, Rhs, PacketScalar, LoadMode>
{
EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, PacketScalar &res)
{
res = ei_pmul(ei_pset1(lhs.coeff(row, 0)),rhs.template packet<LoadMode>(0, col));
}
};
template<typename Lhs, typename Rhs, typename PacketScalar, int LoadMode>
struct ei_product_packet_impl<ColMajor, 0, Lhs, Rhs, PacketScalar, LoadMode>
{
EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, PacketScalar &res)
{
res = ei_pmul(lhs.template packet<LoadMode>(row, 0), ei_pset1(rhs.coeff(0, col)));
}
};
template<typename Lhs, typename Rhs, typename PacketScalar, int LoadMode>
struct ei_product_packet_impl<RowMajor, Dynamic, Lhs, Rhs, PacketScalar, LoadMode>
{
EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, PacketScalar& res)
{
ei_assert(lhs.cols()>0 && "you are using a non initialized matrix");
res = ei_pmul(ei_pset1(lhs.coeff(row, 0)),rhs.template packet<LoadMode>(0, col));
for(int i = 1; i < lhs.cols(); ++i)
res = ei_pmadd(ei_pset1(lhs.coeff(row, i)), rhs.template packet<LoadMode>(i, col), res);
}
};
template<typename Lhs, typename Rhs, typename PacketScalar, int LoadMode>
struct ei_product_packet_impl<ColMajor, Dynamic, Lhs, Rhs, PacketScalar, LoadMode>
{
EIGEN_STRONG_INLINE static void run(int row, int col, const Lhs& lhs, const Rhs& rhs, PacketScalar& res)
{
ei_assert(lhs.cols()>0 && "you are using a non initialized matrix");
res = ei_pmul(lhs.template packet<LoadMode>(row, 0), ei_pset1(rhs.coeff(0, col)));
for(int i = 1; i < lhs.cols(); ++i)
res = ei_pmadd(lhs.template packet<LoadMode>(row, i), ei_pset1(rhs.coeff(i, col)), res);
}
};
/***************************************************************************
* Cache friendly product callers and specific nested evaluation strategies
***************************************************************************/
template<typename Scalar, typename RhsType>
static void ei_cache_friendly_product_colmajor_times_vector(
int size, const Scalar* lhs, int lhsStride, const RhsType& rhs, Scalar* res);
template<typename Scalar, typename ResType>
static void ei_cache_friendly_product_rowmajor_times_vector(
const Scalar* lhs, int lhsStride, const Scalar* rhs, int rhsSize, ResType& res);
template<typename ProductType,
int LhsRows = ei_traits<ProductType>::RowsAtCompileTime,
int LhsOrder = int(ei_traits<ProductType>::LhsFlags)&RowMajorBit ? RowMajor : ColMajor,
int LhsHasDirectAccess = int(ei_traits<ProductType>::LhsFlags)&DirectAccessBit? HasDirectAccess : NoDirectAccess,
int RhsCols = ei_traits<ProductType>::ColsAtCompileTime,
int RhsOrder = int(ei_traits<ProductType>::RhsFlags)&RowMajorBit ? RowMajor : ColMajor,
int RhsHasDirectAccess = int(ei_traits<ProductType>::RhsFlags)&DirectAccessBit? HasDirectAccess : NoDirectAccess>
struct ei_cache_friendly_product_selector
{
template<typename DestDerived>
inline static void run(DestDerived& res, const ProductType& product)
{
product._cacheFriendlyEvalAndAdd(res);
}
};
// optimized colmajor * vector path
template<typename ProductType, int LhsRows, int RhsOrder, int RhsAccess>
struct ei_cache_friendly_product_selector<ProductType,LhsRows,ColMajor,NoDirectAccess,1,RhsOrder,RhsAccess>
{
template<typename DestDerived>
inline static void run(DestDerived& res, const ProductType& product)
{
const int size = product.rhs().rows();
for (int k=0; k<size; ++k)
res += product.rhs().coeff(k) * product.lhs().col(k);
}
};
// optimized cache friendly colmajor * vector path for matrix with direct access flag
// NOTE this path could also be enabled for expressions if we add runtime align queries
template<typename ProductType, int LhsRows, int RhsOrder, int RhsAccess>
struct ei_cache_friendly_product_selector<ProductType,LhsRows,ColMajor,HasDirectAccess,1,RhsOrder,RhsAccess>
{
typedef typename ProductType::Scalar Scalar;
template<typename DestDerived>
inline static void run(DestDerived& res, const ProductType& product)
{
enum {
EvalToRes = (ei_packet_traits<Scalar>::size==1)
||((DestDerived::Flags&ActualPacketAccessBit) && (!(DestDerived::Flags & RowMajorBit))) };
Scalar* EIGEN_RESTRICT _res;
if (EvalToRes)
_res = &res.coeffRef(0);
else
{
_res = ei_aligned_stack_new(Scalar,res.size());
Map<Matrix<Scalar,DestDerived::RowsAtCompileTime,1,ColMajor> >(_res, res.size()) = res;
}
ei_cache_friendly_product_colmajor_times_vector(res.size(),
&product.lhs().const_cast_derived().coeffRef(0,0), product.lhs().stride(),
product.rhs(), _res);
if (!EvalToRes)
{
res = Map<Matrix<Scalar,DestDerived::SizeAtCompileTime,1,ColMajor> >(_res, res.size());
ei_aligned_stack_delete(Scalar, _res, res.size());
}
}
};
// optimized vector * rowmajor path
template<typename ProductType, int LhsOrder, int LhsAccess, int RhsCols>
struct ei_cache_friendly_product_selector<ProductType,1,LhsOrder,LhsAccess,RhsCols,RowMajor,NoDirectAccess>
{
template<typename DestDerived>
inline static void run(DestDerived& res, const ProductType& product)
{
const int cols = product.lhs().cols();
for (int j=0; j<cols; ++j)
res += product.lhs().coeff(j) * product.rhs().row(j);
}
};
// optimized cache friendly vector * rowmajor path for matrix with direct access flag
// NOTE this path coul also be enabled for expressions if we add runtime align queries
template<typename ProductType, int LhsOrder, int LhsAccess, int RhsCols>
struct ei_cache_friendly_product_selector<ProductType,1,LhsOrder,LhsAccess,RhsCols,RowMajor,HasDirectAccess>
{
typedef typename ProductType::Scalar Scalar;
template<typename DestDerived>
inline static void run(DestDerived& res, const ProductType& product)
{
enum {
EvalToRes = (ei_packet_traits<Scalar>::size==1)
||((DestDerived::Flags & ActualPacketAccessBit) && (DestDerived::Flags & RowMajorBit)) };
Scalar* EIGEN_RESTRICT _res;
if (EvalToRes)
_res = &res.coeffRef(0);
else
{
_res = ei_aligned_stack_new(Scalar, res.size());
Map<Matrix<Scalar,1,DestDerived::SizeAtCompileTime,ColMajor> >(_res, res.size()) = res;
}
ei_cache_friendly_product_colmajor_times_vector(res.size(),
&product.rhs().const_cast_derived().coeffRef(0,0), product.rhs().stride(),
product.lhs().transpose(), _res);
if (!EvalToRes)
{
res = Map<Matrix<Scalar,1,DestDerived::SizeAtCompileTime,ColMajor> >(_res, res.size());
ei_aligned_stack_delete(Scalar, _res, res.size());
}
}
};
// optimized rowmajor - vector product
template<typename ProductType, int LhsRows, int RhsOrder, int RhsAccess>
struct ei_cache_friendly_product_selector<ProductType,LhsRows,RowMajor,HasDirectAccess,1,RhsOrder,RhsAccess>
{
typedef typename ProductType::Scalar Scalar;
typedef typename ei_traits<ProductType>::_RhsNested Rhs;
enum {
UseRhsDirectly = ((ei_packet_traits<Scalar>::size==1) || (Rhs::Flags&ActualPacketAccessBit))
&& (Rhs::Flags&DirectAccessBit)
&& (!(Rhs::Flags & RowMajorBit)) };
template<typename DestDerived>
inline static void run(DestDerived& res, const ProductType& product)
{
Scalar* EIGEN_RESTRICT _rhs;
if (UseRhsDirectly)
_rhs = &product.rhs().const_cast_derived().coeffRef(0);
else
{
_rhs = ei_aligned_stack_new(Scalar, product.rhs().size());
Map<Matrix<Scalar,Rhs::SizeAtCompileTime,1,ColMajor> >(_rhs, product.rhs().size()) = product.rhs();
}
ei_cache_friendly_product_rowmajor_times_vector(&product.lhs().const_cast_derived().coeffRef(0,0), product.lhs().stride(),
_rhs, product.rhs().size(), res);
if (!UseRhsDirectly) ei_aligned_stack_delete(Scalar, _rhs, product.rhs().size());
}
};
// optimized vector - colmajor product
template<typename ProductType, int LhsOrder, int LhsAccess, int RhsCols>
struct ei_cache_friendly_product_selector<ProductType,1,LhsOrder,LhsAccess,RhsCols,ColMajor,HasDirectAccess>
{
typedef typename ProductType::Scalar Scalar;
typedef typename ei_traits<ProductType>::_LhsNested Lhs;
enum {
UseLhsDirectly = ((ei_packet_traits<Scalar>::size==1) || (Lhs::Flags&ActualPacketAccessBit))
&& (Lhs::Flags&DirectAccessBit)
&& (Lhs::Flags & RowMajorBit) };
template<typename DestDerived>
inline static void run(DestDerived& res, const ProductType& product)
{
Scalar* EIGEN_RESTRICT _lhs;
if (UseLhsDirectly)
_lhs = &product.lhs().const_cast_derived().coeffRef(0);
else
{
_lhs = ei_aligned_stack_new(Scalar, product.lhs().size());
Map<Matrix<Scalar,1,Lhs::SizeAtCompileTime,ColMajor> >(_lhs, product.lhs().size()) = product.lhs();
}
ei_cache_friendly_product_rowmajor_times_vector(&product.rhs().const_cast_derived().coeffRef(0,0), product.rhs().stride(),
_lhs, product.lhs().size(), res);
if(!UseLhsDirectly) ei_aligned_stack_delete(Scalar, _lhs, product.lhs().size());
}
};
// discard this case which has to be handled by the default path
// (we keep it to be sure to hit a compilation error if this is not the case)
template<typename ProductType, int LhsRows, int RhsOrder, int RhsAccess>
struct ei_cache_friendly_product_selector<ProductType,LhsRows,RowMajor,NoDirectAccess,1,RhsOrder,RhsAccess>
{};
// discard this case which has to be handled by the default path
// (we keep it to be sure to hit a compilation error if this is not the case)
template<typename ProductType, int LhsOrder, int LhsAccess, int RhsCols>
struct ei_cache_friendly_product_selector<ProductType,1,LhsOrder,LhsAccess,RhsCols,ColMajor,NoDirectAccess>
{};
/** \internal */
template<typename Derived>
template<typename Lhs,typename Rhs>
inline Derived&
MatrixBase<Derived>::operator+=(const Flagged<Product<Lhs,Rhs,CacheFriendlyProduct>, 0, EvalBeforeNestingBit | EvalBeforeAssigningBit>& other)
{
if (other._expression()._useCacheFriendlyProduct())
ei_cache_friendly_product_selector<Product<Lhs,Rhs,CacheFriendlyProduct> >::run(const_cast_derived(), other._expression());
else
{
typedef typename ei_cleantype<Lhs>::type _Lhs;
typedef typename ei_cleantype<Rhs>::type _Rhs;
typedef typename ei_nested<_Lhs,_Rhs::ColsAtCompileTime>::type LhsNested;
typedef typename ei_nested<_Rhs,_Lhs::RowsAtCompileTime>::type RhsNested;
Product<LhsNested,RhsNested,NormalProduct> prod(other._expression().lhs(),other._expression().rhs());
lazyAssign(derived() + prod);
}
return derived();
}
template<typename Derived>
template<typename Lhs, typename Rhs>
inline Derived& MatrixBase<Derived>::lazyAssign(const Product<Lhs,Rhs,CacheFriendlyProduct>& product)
{
if (product._useCacheFriendlyProduct())
{
setZero();
ei_cache_friendly_product_selector<Product<Lhs,Rhs,CacheFriendlyProduct> >::run(const_cast_derived(), product);
}
else
{
typedef typename ei_cleantype<Lhs>::type _Lhs;
typedef typename ei_cleantype<Rhs>::type _Rhs;
typedef typename ei_nested<_Lhs,_Rhs::ColsAtCompileTime>::type LhsNested;
typedef typename ei_nested<_Rhs,_Lhs::RowsAtCompileTime>::type RhsNested;
typedef Product<LhsNested,RhsNested,NormalProduct> NormalProduct;
NormalProduct normal_prod(product.lhs(),product.rhs());
lazyAssign<NormalProduct>(normal_prod);
}
return derived();
}
template<typename T,int StorageOrder> struct ei_product_copy_rhs
{
typedef typename ei_meta_if<
(ei_traits<T>::Flags & RowMajorBit)
|| (!(ei_traits<T>::Flags & DirectAccessBit)),
typename ei_plain_matrix_type_column_major<T>::type,
const T&
>::ret type;
};
template<typename T> struct ei_product_copy_rhs<T,RowMajorBit>
{
typedef typename ei_meta_if<
(!(ei_traits<T>::Flags & DirectAccessBit)),
typename ei_plain_matrix_type<T>::type,
const T&
>::ret type;
};
template<typename T,int StorageOrder> struct ei_product_copy_lhs
{
typedef typename ei_meta_if<
(!(int(ei_traits<T>::Flags) & DirectAccessBit)),
typename ei_plain_matrix_type_row_major<T>::type,
const T&
>::ret type;
};
template<typename T> struct ei_product_copy_lhs<T,RowMajorBit>
{
typedef typename ei_meta_if<
((ei_traits<T>::Flags & RowMajorBit)==0)
|| (!(int(ei_traits<T>::Flags) & DirectAccessBit)),
typename ei_plain_matrix_type_row_major<T>::type,
const T&
>::ret type;
};
template<typename Lhs, typename Rhs, int ProductMode>
template<typename DestDerived>
inline void Product<Lhs,Rhs,ProductMode>::_cacheFriendlyEvalAndAdd(DestDerived& res) const
{
typedef typename ei_product_copy_lhs<_LhsNested,DestDerived::Flags&RowMajorBit>::type LhsCopy;
typedef typename ei_unref<LhsCopy>::type _LhsCopy;
typedef typename ei_product_copy_rhs<_RhsNested,DestDerived::Flags&RowMajorBit>::type RhsCopy;
typedef typename ei_unref<RhsCopy>::type _RhsCopy;
LhsCopy lhs(m_lhs);
RhsCopy rhs(m_rhs);
ei_cache_friendly_product<Scalar>(
rows(), cols(), lhs.cols(),
_LhsCopy::Flags&RowMajorBit, (const Scalar*)&(lhs.const_cast_derived().coeffRef(0,0)), lhs.stride(),
_RhsCopy::Flags&RowMajorBit, (const Scalar*)&(rhs.const_cast_derived().coeffRef(0,0)), rhs.stride(),
DestDerived::Flags&RowMajorBit, (Scalar*)&(res.coeffRef(0,0)), res.stride()
);
}
#endif // EIGEN_PRODUCT_H

117
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Redux.h Normal file
View File

@ -0,0 +1,117 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_REDUX_H
#define EIGEN_REDUX_H
template<typename BinaryOp, typename Derived, int Start, int Length>
struct ei_redux_impl
{
enum {
HalfLength = Length/2
};
typedef typename ei_result_of<BinaryOp(typename Derived::Scalar)>::type Scalar;
static Scalar run(const Derived &mat, const BinaryOp& func)
{
return func(
ei_redux_impl<BinaryOp, Derived, Start, HalfLength>::run(mat, func),
ei_redux_impl<BinaryOp, Derived, Start+HalfLength, Length - HalfLength>::run(mat, func));
}
};
template<typename BinaryOp, typename Derived, int Start>
struct ei_redux_impl<BinaryOp, Derived, Start, 1>
{
enum {
col = Start / Derived::RowsAtCompileTime,
row = Start % Derived::RowsAtCompileTime
};
typedef typename ei_result_of<BinaryOp(typename Derived::Scalar)>::type Scalar;
static Scalar run(const Derived &mat, const BinaryOp &)
{
return mat.coeff(row, col);
}
};
template<typename BinaryOp, typename Derived, int Start>
struct ei_redux_impl<BinaryOp, Derived, Start, Dynamic>
{
typedef typename ei_result_of<BinaryOp(typename Derived::Scalar)>::type Scalar;
static Scalar run(const Derived& mat, const BinaryOp& func)
{
ei_assert(mat.rows()>0 && mat.cols()>0 && "you are using a non initialized matrix");
Scalar res;
res = mat.coeff(0,0);
for(int i = 1; i < mat.rows(); ++i)
res = func(res, mat.coeff(i, 0));
for(int j = 1; j < mat.cols(); ++j)
for(int i = 0; i < mat.rows(); ++i)
res = func(res, mat.coeff(i, j));
return res;
}
};
/** \returns the result of a full redux operation on the whole matrix or vector using \a func
*
* The template parameter \a BinaryOp is the type of the functor \a func which must be
* an assiociative operator. Both current STL and TR1 functor styles are handled.
*
* \sa MatrixBase::sum(), MatrixBase::minCoeff(), MatrixBase::maxCoeff(), MatrixBase::colwise(), MatrixBase::rowwise()
*/
template<typename Derived>
template<typename BinaryOp>
typename ei_result_of<BinaryOp(typename ei_traits<Derived>::Scalar)>::type
MatrixBase<Derived>::redux(const BinaryOp& func) const
{
const bool unroll = SizeAtCompileTime * CoeffReadCost
+ (SizeAtCompileTime-1) * ei_functor_traits<BinaryOp>::Cost
<= EIGEN_UNROLLING_LIMIT;
return ei_redux_impl<BinaryOp, Derived, 0, unroll ? int(SizeAtCompileTime) : Dynamic>
::run(derived(), func);
}
/** \returns the minimum of all coefficients of *this
*/
template<typename Derived>
inline typename ei_traits<Derived>::Scalar
MatrixBase<Derived>::minCoeff() const
{
return this->redux(Eigen::ei_scalar_min_op<Scalar>());
}
/** \returns the maximum of all coefficients of *this
*/
template<typename Derived>
inline typename ei_traits<Derived>::Scalar
MatrixBase<Derived>::maxCoeff() const
{
return this->redux(Eigen::ei_scalar_max_op<Scalar>());
}
#endif // EIGEN_REDUX_H

297
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/SolveTriangular.h Normal file
View File

@ -0,0 +1,297 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_SOLVETRIANGULAR_H
#define EIGEN_SOLVETRIANGULAR_H
template<typename XprType> struct ei_is_part { enum {value=false}; };
template<typename XprType, unsigned int Mode> struct ei_is_part<Part<XprType,Mode> > { enum {value=true}; };
template<typename Lhs, typename Rhs,
int TriangularPart = (int(Lhs::Flags) & LowerTriangularBit)
? LowerTriangular
: (int(Lhs::Flags) & UpperTriangularBit)
? UpperTriangular
: -1,
int StorageOrder = ei_is_part<Lhs>::value ? -1 // this is to solve ambiguous specializations
: int(Lhs::Flags) & int(RowMajorBit|SparseBit)
>
struct ei_solve_triangular_selector;
// transform a Part xpr to a Flagged xpr
template<typename Lhs, unsigned int LhsMode, typename Rhs, int UpLo, int StorageOrder>
struct ei_solve_triangular_selector<Part<Lhs,LhsMode>,Rhs,UpLo,StorageOrder>
{
static void run(const Part<Lhs,LhsMode>& lhs, Rhs& other)
{
ei_solve_triangular_selector<Flagged<Lhs,LhsMode,0>,Rhs>::run(lhs._expression(), other);
}
};
// forward substitution, row-major
template<typename Lhs, typename Rhs, int UpLo>
struct ei_solve_triangular_selector<Lhs,Rhs,UpLo,RowMajor|IsDense>
{
typedef typename Rhs::Scalar Scalar;
static void run(const Lhs& lhs, Rhs& other)
{
const bool IsLowerTriangular = (UpLo==LowerTriangular);
const int size = lhs.cols();
/* We perform the inverse product per block of 4 rows such that we perfectly match
* our optimized matrix * vector product. blockyStart represents the number of rows
* we have process first using the non-block version.
*/
int blockyStart = (std::max(size-5,0)/4)*4;
if (IsLowerTriangular)
blockyStart = size - blockyStart;
else
blockyStart -= 1;
for(int c=0 ; c<other.cols() ; ++c)
{
// process first rows using the non block version
if(!(Lhs::Flags & UnitDiagBit))
{
if (IsLowerTriangular)
other.coeffRef(0,c) = other.coeff(0,c)/lhs.coeff(0, 0);
else
other.coeffRef(size-1,c) = other.coeff(size-1, c)/lhs.coeff(size-1, size-1);
}
for(int i=(IsLowerTriangular ? 1 : size-2); IsLowerTriangular ? i<blockyStart : i>blockyStart; i += (IsLowerTriangular ? 1 : -1) )
{
Scalar tmp = other.coeff(i,c)
- (IsLowerTriangular ? ((lhs.row(i).start(i)) * other.col(c).start(i)).coeff(0,0)
: ((lhs.row(i).end(size-i-1)) * other.col(c).end(size-i-1)).coeff(0,0));
if (Lhs::Flags & UnitDiagBit)
other.coeffRef(i,c) = tmp;
else
other.coeffRef(i,c) = tmp/lhs.coeff(i,i);
}
// now let's process the remaining rows 4 at once
for(int i=blockyStart; IsLowerTriangular ? i<size : i>0; )
{
int startBlock = i;
int endBlock = startBlock + (IsLowerTriangular ? 4 : -4);
/* Process the i cols times 4 rows block, and keep the result in a temporary vector */
// FIXME use fixed size block but take care to small fixed size matrices...
Matrix<Scalar,Dynamic,1> btmp(4);
if (IsLowerTriangular)
btmp = lhs.block(startBlock,0,4,i) * other.col(c).start(i);
else
btmp = lhs.block(i-3,i+1,4,size-1-i) * other.col(c).end(size-1-i);
/* Let's process the 4x4 sub-matrix as usual.
* btmp stores the diagonal coefficients used to update the remaining part of the result.
*/
{
Scalar tmp = other.coeff(startBlock,c)-btmp.coeff(IsLowerTriangular?0:3);
if (Lhs::Flags & UnitDiagBit)
other.coeffRef(i,c) = tmp;
else
other.coeffRef(i,c) = tmp/lhs.coeff(i,i);
}
i += IsLowerTriangular ? 1 : -1;
for (;IsLowerTriangular ? i<endBlock : i>endBlock; i += IsLowerTriangular ? 1 : -1)
{
int remainingSize = IsLowerTriangular ? i-startBlock : startBlock-i;
Scalar tmp = other.coeff(i,c)
- btmp.coeff(IsLowerTriangular ? remainingSize : 3-remainingSize)
- ( lhs.row(i).segment(IsLowerTriangular ? startBlock : i+1, remainingSize)
* other.col(c).segment(IsLowerTriangular ? startBlock : i+1, remainingSize)).coeff(0,0);
if (Lhs::Flags & UnitDiagBit)
other.coeffRef(i,c) = tmp;
else
other.coeffRef(i,c) = tmp/lhs.coeff(i,i);
}
}
}
}
};
// Implements the following configurations:
// - inv(LowerTriangular, ColMajor) * Column vector
// - inv(LowerTriangular,UnitDiag,ColMajor) * Column vector
// - inv(UpperTriangular, ColMajor) * Column vector
// - inv(UpperTriangular,UnitDiag,ColMajor) * Column vector
template<typename Lhs, typename Rhs, int UpLo>
struct ei_solve_triangular_selector<Lhs,Rhs,UpLo,ColMajor|IsDense>
{
typedef typename Rhs::Scalar Scalar;
typedef typename ei_packet_traits<Scalar>::type Packet;
enum { PacketSize = ei_packet_traits<Scalar>::size };
static void run(const Lhs& lhs, Rhs& other)
{
static const bool IsLowerTriangular = (UpLo==LowerTriangular);
const int size = lhs.cols();
for(int c=0 ; c<other.cols() ; ++c)
{
/* let's perform the inverse product per block of 4 columns such that we perfectly match
* our optimized matrix * vector product. blockyEnd represents the number of rows
* we can process using the block version.
*/
int blockyEnd = (std::max(size-5,0)/4)*4;
if (!IsLowerTriangular)
blockyEnd = size-1 - blockyEnd;
for(int i=IsLowerTriangular ? 0 : size-1; IsLowerTriangular ? i<blockyEnd : i>blockyEnd;)
{
/* Let's process the 4x4 sub-matrix as usual.
* btmp stores the diagonal coefficients used to update the remaining part of the result.
*/
int startBlock = i;
int endBlock = startBlock + (IsLowerTriangular ? 4 : -4);
Matrix<Scalar,4,1> btmp;
for (;IsLowerTriangular ? i<endBlock : i>endBlock;
i += IsLowerTriangular ? 1 : -1)
{
if(!(Lhs::Flags & UnitDiagBit))
other.coeffRef(i,c) /= lhs.coeff(i,i);
int remainingSize = IsLowerTriangular ? endBlock-i-1 : i-endBlock-1;
if (remainingSize>0)
other.col(c).segment((IsLowerTriangular ? i : endBlock) + 1, remainingSize) -=
other.coeffRef(i,c)
* Block<Lhs,Dynamic,1>(lhs, (IsLowerTriangular ? i : endBlock) + 1, i, remainingSize, 1);
btmp.coeffRef(IsLowerTriangular ? i-startBlock : remainingSize) = -other.coeffRef(i,c);
}
/* Now we can efficiently update the remaining part of the result as a matrix * vector product.
* NOTE in order to reduce both compilation time and binary size, let's directly call
* the fast product implementation. It is equivalent to the following code:
* other.col(c).end(size-endBlock) += (lhs.block(endBlock, startBlock, size-endBlock, endBlock-startBlock)
* * other.col(c).block(startBlock,endBlock-startBlock)).lazy();
*/
// FIXME this is cool but what about conjugate/adjoint expressions ? do we want to evaluate them ?
// this is a more general problem though.
ei_cache_friendly_product_colmajor_times_vector(
IsLowerTriangular ? size-endBlock : endBlock+1,
&(lhs.const_cast_derived().coeffRef(IsLowerTriangular ? endBlock : 0, IsLowerTriangular ? startBlock : endBlock+1)),
lhs.stride(),
btmp, &(other.coeffRef(IsLowerTriangular ? endBlock : 0, c)));
// if (IsLowerTriangular)
// other.col(c).end(size-endBlock) += (lhs.block(endBlock, startBlock, size-endBlock, endBlock-startBlock)
// * other.col(c).block(startBlock,endBlock-startBlock)).lazy();
// else
// other.col(c).end(size-endBlock) += (lhs.block(endBlock, startBlock, size-endBlock, endBlock-startBlock)
// * other.col(c).block(startBlock,endBlock-startBlock)).lazy();
}
/* Now we have to process the remaining part as usual */
int i;
for(i=blockyEnd; IsLowerTriangular ? i<size-1 : i>0; i += (IsLowerTriangular ? 1 : -1) )
{
if(!(Lhs::Flags & UnitDiagBit))
other.coeffRef(i,c) /= lhs.coeff(i,i);
/* NOTE we cannot use lhs.col(i).end(size-i-1) because Part::coeffRef gets called by .col() to
* get the address of the start of the row
*/
if(IsLowerTriangular)
other.col(c).end(size-i-1) -= other.coeffRef(i,c) * Block<Lhs,Dynamic,1>(lhs, i+1,i, size-i-1,1);
else
other.col(c).start(i) -= other.coeffRef(i,c) * Block<Lhs,Dynamic,1>(lhs, 0,i, i, 1);
}
if(!(Lhs::Flags & UnitDiagBit))
other.coeffRef(i,c) /= lhs.coeff(i,i);
}
}
};
/** "in-place" version of MatrixBase::solveTriangular() where the result is written in \a other
*
* \nonstableyet
*
* The parameter is only marked 'const' to make the C++ compiler accept a temporary expression here.
* This function will const_cast it, so constness isn't honored here.
*
* See MatrixBase:solveTriangular() for the details.
*/
template<typename Derived>
template<typename OtherDerived>
void MatrixBase<Derived>::solveTriangularInPlace(const MatrixBase<OtherDerived>& _other) const
{
MatrixBase<OtherDerived>& other = _other.const_cast_derived();
ei_assert(derived().cols() == derived().rows());
ei_assert(derived().cols() == other.rows());
ei_assert(!(Flags & ZeroDiagBit));
ei_assert(Flags & (UpperTriangularBit|LowerTriangularBit));
enum { copy = ei_traits<OtherDerived>::Flags & RowMajorBit };
typedef typename ei_meta_if<copy,
typename ei_plain_matrix_type_column_major<OtherDerived>::type, OtherDerived&>::ret OtherCopy;
OtherCopy otherCopy(other.derived());
ei_solve_triangular_selector<Derived, typename ei_unref<OtherCopy>::type>::run(derived(), otherCopy);
if (copy)
other = otherCopy;
}
/** \returns the product of the inverse of \c *this with \a other, \a *this being triangular.
*
* \nonstableyet
*
* This function computes the inverse-matrix matrix product inverse(\c *this) * \a other.
* The matrix \c *this must be triangular and invertible (i.e., all the coefficients of the
* diagonal must be non zero). It works as a forward (resp. backward) substitution if \c *this
* is an upper (resp. lower) triangular matrix.
*
* It is required that \c *this be marked as either an upper or a lower triangular matrix, which
* can be done by marked(), and that is automatically the case with expressions such as those returned
* by extract().
*
* \addexample SolveTriangular \label How to solve a triangular system (aka. how to multiply the inverse of a triangular matrix by another one)
*
* Example: \include MatrixBase_marked.cpp
* Output: \verbinclude MatrixBase_marked.out
*
* This function is essentially a wrapper to the faster solveTriangularInPlace() function creating
* a temporary copy of \a other, calling solveTriangularInPlace() on the copy and returning it.
* Therefore, if \a other is not needed anymore, it is quite faster to call solveTriangularInPlace()
* instead of solveTriangular().
*
* For users coming from BLAS, this function (and more specifically solveTriangularInPlace()) offer
* all the operations supported by the \c *TRSV and \c *TRSM BLAS routines.
*
* \b Tips: to perform a \em "right-inverse-multiply" you can simply transpose the operation, e.g.:
* \code
* M * T^1 <=> T.transpose().solveTriangularInPlace(M.transpose());
* \endcode
*
* \sa solveTriangularInPlace(), marked(), extract()
*/
template<typename Derived>
template<typename OtherDerived>
typename ei_plain_matrix_type_column_major<OtherDerived>::type
MatrixBase<Derived>::solveTriangular(const MatrixBase<OtherDerived>& other) const
{
typename ei_plain_matrix_type_column_major<OtherDerived>::type res(other);
solveTriangularInPlace(res);
return res;
}
#endif // EIGEN_SOLVETRIANGULAR_H

271
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Sum.h Normal file
View File

@ -0,0 +1,271 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_SUM_H
#define EIGEN_SUM_H
/***************************************************************************
* Part 1 : the logic deciding a strategy for vectorization and unrolling
***************************************************************************/
template<typename Derived>
struct ei_sum_traits
{
private:
enum {
PacketSize = ei_packet_traits<typename Derived::Scalar>::size
};
public:
enum {
Vectorization = (int(Derived::Flags)&ActualPacketAccessBit)
&& (int(Derived::Flags)&LinearAccessBit)
? LinearVectorization
: NoVectorization
};
private:
enum {
Cost = Derived::SizeAtCompileTime * Derived::CoeffReadCost
+ (Derived::SizeAtCompileTime-1) * NumTraits<typename Derived::Scalar>::AddCost,
UnrollingLimit = EIGEN_UNROLLING_LIMIT * (int(Vectorization) == int(NoVectorization) ? 1 : int(PacketSize))
};
public:
enum {
Unrolling = Cost <= UnrollingLimit
? CompleteUnrolling
: NoUnrolling
};
};
/***************************************************************************
* Part 2 : unrollers
***************************************************************************/
/*** no vectorization ***/
template<typename Derived, int Start, int Length>
struct ei_sum_novec_unroller
{
enum {
HalfLength = Length/2
};
typedef typename Derived::Scalar Scalar;
inline static Scalar run(const Derived &mat)
{
return ei_sum_novec_unroller<Derived, Start, HalfLength>::run(mat)
+ ei_sum_novec_unroller<Derived, Start+HalfLength, Length-HalfLength>::run(mat);
}
};
template<typename Derived, int Start>
struct ei_sum_novec_unroller<Derived, Start, 1>
{
enum {
col = Start / Derived::RowsAtCompileTime,
row = Start % Derived::RowsAtCompileTime
};
typedef typename Derived::Scalar Scalar;
inline static Scalar run(const Derived &mat)
{
return mat.coeff(row, col);
}
};
/*** vectorization ***/
template<typename Derived, int Start, int Length>
struct ei_sum_vec_unroller
{
enum {
PacketSize = ei_packet_traits<typename Derived::Scalar>::size,
HalfLength = Length/2
};
typedef typename Derived::Scalar Scalar;
typedef typename ei_packet_traits<Scalar>::type PacketScalar;
inline static PacketScalar run(const Derived &mat)
{
return ei_padd(
ei_sum_vec_unroller<Derived, Start, HalfLength>::run(mat),
ei_sum_vec_unroller<Derived, Start+HalfLength, Length-HalfLength>::run(mat) );
}
};
template<typename Derived, int Start>
struct ei_sum_vec_unroller<Derived, Start, 1>
{
enum {
index = Start * ei_packet_traits<typename Derived::Scalar>::size,
row = int(Derived::Flags)&RowMajorBit
? index / int(Derived::ColsAtCompileTime)
: index % Derived::RowsAtCompileTime,
col = int(Derived::Flags)&RowMajorBit
? index % int(Derived::ColsAtCompileTime)
: index / Derived::RowsAtCompileTime,
alignment = (Derived::Flags & AlignedBit) ? Aligned : Unaligned
};
typedef typename Derived::Scalar Scalar;
typedef typename ei_packet_traits<Scalar>::type PacketScalar;
inline static PacketScalar run(const Derived &mat)
{
return mat.template packet<alignment>(row, col);
}
};
/***************************************************************************
* Part 3 : implementation of all cases
***************************************************************************/
template<typename Derived,
int Vectorization = ei_sum_traits<Derived>::Vectorization,
int Unrolling = ei_sum_traits<Derived>::Unrolling
>
struct ei_sum_impl;
template<typename Derived>
struct ei_sum_impl<Derived, NoVectorization, NoUnrolling>
{
typedef typename Derived::Scalar Scalar;
static Scalar run(const Derived& mat)
{
ei_assert(mat.rows()>0 && mat.cols()>0 && "you are using a non initialized matrix");
Scalar res;
res = mat.coeff(0, 0);
for(int i = 1; i < mat.rows(); ++i)
res += mat.coeff(i, 0);
for(int j = 1; j < mat.cols(); ++j)
for(int i = 0; i < mat.rows(); ++i)
res += mat.coeff(i, j);
return res;
}
};
template<typename Derived>
struct ei_sum_impl<Derived, NoVectorization, CompleteUnrolling>
: public ei_sum_novec_unroller<Derived, 0, Derived::SizeAtCompileTime>
{};
template<typename Derived>
struct ei_sum_impl<Derived, LinearVectorization, NoUnrolling>
{
typedef typename Derived::Scalar Scalar;
typedef typename ei_packet_traits<Scalar>::type PacketScalar;
static Scalar run(const Derived& mat)
{
const int size = mat.size();
const int packetSize = ei_packet_traits<Scalar>::size;
const int alignedStart = (Derived::Flags & AlignedBit)
|| !(Derived::Flags & DirectAccessBit)
? 0
: ei_alignmentOffset(&mat.const_cast_derived().coeffRef(0), size);
enum {
alignment = (Derived::Flags & DirectAccessBit) || (Derived::Flags & AlignedBit)
? Aligned : Unaligned
};
const int alignedSize = ((size-alignedStart)/packetSize)*packetSize;
const int alignedEnd = alignedStart + alignedSize;
Scalar res;
if(alignedSize)
{
PacketScalar packet_res = mat.template packet<alignment>(alignedStart);
for(int index = alignedStart + packetSize; index < alignedEnd; index += packetSize)
packet_res = ei_padd(packet_res, mat.template packet<alignment>(index));
res = ei_predux(packet_res);
}
else // too small to vectorize anything.
// since this is dynamic-size hence inefficient anyway for such small sizes, don't try to optimize.
{
res = Scalar(0);
}
for(int index = 0; index < alignedStart; ++index)
res += mat.coeff(index);
for(int index = alignedEnd; index < size; ++index)
res += mat.coeff(index);
return res;
}
};
template<typename Derived>
struct ei_sum_impl<Derived, LinearVectorization, CompleteUnrolling>
{
typedef typename Derived::Scalar Scalar;
typedef typename ei_packet_traits<Scalar>::type PacketScalar;
enum {
PacketSize = ei_packet_traits<Scalar>::size,
Size = Derived::SizeAtCompileTime,
VectorizationSize = (Size / PacketSize) * PacketSize
};
static Scalar run(const Derived& mat)
{
Scalar res = ei_predux(ei_sum_vec_unroller<Derived, 0, Size / PacketSize>::run(mat));
if (VectorizationSize != Size)
res += ei_sum_novec_unroller<Derived, VectorizationSize, Size-VectorizationSize>::run(mat);
return res;
}
};
/***************************************************************************
* Part 4 : implementation of MatrixBase methods
***************************************************************************/
/** \returns the sum of all coefficients of *this
*
* \sa trace()
*/
template<typename Derived>
inline typename ei_traits<Derived>::Scalar
MatrixBase<Derived>::sum() const
{
return ei_sum_impl<Derived>::run(derived());
}
/** \returns the trace of \c *this, i.e. the sum of the coefficients on the main diagonal.
*
* \c *this can be any matrix, not necessarily square.
*
* \sa diagonal(), sum()
*/
template<typename Derived>
inline typename ei_traits<Derived>::Scalar
MatrixBase<Derived>::trace() const
{
return diagonal().sum();
}
#endif // EIGEN_SUM_H

145
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Swap.h Normal file
View File

@ -0,0 +1,145 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_SWAP_H
#define EIGEN_SWAP_H
/** \class SwapWrapper
*
* \internal
*
* \brief Internal helper class for swapping two expressions
*/
template<typename ExpressionType>
struct ei_traits<SwapWrapper<ExpressionType> >
{
typedef typename ExpressionType::Scalar Scalar;
enum {
RowsAtCompileTime = ExpressionType::RowsAtCompileTime,
ColsAtCompileTime = ExpressionType::ColsAtCompileTime,
MaxRowsAtCompileTime = ExpressionType::MaxRowsAtCompileTime,
MaxColsAtCompileTime = ExpressionType::MaxColsAtCompileTime,
Flags = ExpressionType::Flags,
CoeffReadCost = ExpressionType::CoeffReadCost
};
};
template<typename ExpressionType> class SwapWrapper
: public MatrixBase<SwapWrapper<ExpressionType> >
{
public:
EIGEN_GENERIC_PUBLIC_INTERFACE(SwapWrapper)
typedef typename ei_packet_traits<Scalar>::type Packet;
inline SwapWrapper(ExpressionType& xpr) : m_expression(xpr) {}
inline int rows() const { return m_expression.rows(); }
inline int cols() const { return m_expression.cols(); }
inline int stride() const { return m_expression.stride(); }
inline Scalar& coeffRef(int row, int col)
{
return m_expression.const_cast_derived().coeffRef(row, col);
}
inline Scalar& coeffRef(int index)
{
return m_expression.const_cast_derived().coeffRef(index);
}
template<typename OtherDerived>
void copyCoeff(int row, int col, const MatrixBase<OtherDerived>& other)
{
OtherDerived& _other = other.const_cast_derived();
ei_internal_assert(row >= 0 && row < rows()
&& col >= 0 && col < cols());
Scalar tmp = m_expression.coeff(row, col);
m_expression.coeffRef(row, col) = _other.coeff(row, col);
_other.coeffRef(row, col) = tmp;
}
template<typename OtherDerived>
void copyCoeff(int index, const MatrixBase<OtherDerived>& other)
{
OtherDerived& _other = other.const_cast_derived();
ei_internal_assert(index >= 0 && index < m_expression.size());
Scalar tmp = m_expression.coeff(index);
m_expression.coeffRef(index) = _other.coeff(index);
_other.coeffRef(index) = tmp;
}
template<typename OtherDerived, int StoreMode, int LoadMode>
void copyPacket(int row, int col, const MatrixBase<OtherDerived>& other)
{
OtherDerived& _other = other.const_cast_derived();
ei_internal_assert(row >= 0 && row < rows()
&& col >= 0 && col < cols());
Packet tmp = m_expression.template packet<StoreMode>(row, col);
m_expression.template writePacket<StoreMode>(row, col,
_other.template packet<LoadMode>(row, col)
);
_other.template writePacket<LoadMode>(row, col, tmp);
}
template<typename OtherDerived, int StoreMode, int LoadMode>
void copyPacket(int index, const MatrixBase<OtherDerived>& other)
{
OtherDerived& _other = other.const_cast_derived();
ei_internal_assert(index >= 0 && index < m_expression.size());
Packet tmp = m_expression.template packet<StoreMode>(index);
m_expression.template writePacket<StoreMode>(index,
_other.template packet<LoadMode>(index)
);
_other.template writePacket<LoadMode>(index, tmp);
}
protected:
ExpressionType& m_expression;
private:
SwapWrapper& operator=(const SwapWrapper&);
};
/** swaps *this with the expression \a other.
*
* \note \a other is only marked for internal reasons, but of course
* it gets const-casted. One reason is that one will often call swap
* on temporary objects (hence non-const references are forbidden).
* Another reason is that lazyAssign takes a const argument anyway.
*/
template<typename Derived>
template<typename OtherDerived>
void MatrixBase<Derived>::swap(const MatrixBase<OtherDerived>& other)
{
(SwapWrapper<Derived>(derived())).lazyAssign(other);
}
#endif // EIGEN_SWAP_H

227
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Transpose.h Normal file
View File

@ -0,0 +1,227 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_TRANSPOSE_H
#define EIGEN_TRANSPOSE_H
/** \class Transpose
*
* \brief Expression of the transpose of a matrix
*
* \param MatrixType the type of the object of which we are taking the transpose
*
* This class represents an expression of the transpose of a matrix.
* It is the return type of MatrixBase::transpose() and MatrixBase::adjoint()
* and most of the time this is the only way it is used.
*
* \sa MatrixBase::transpose(), MatrixBase::adjoint()
*/
template<typename MatrixType>
struct ei_traits<Transpose<MatrixType> >
{
typedef typename MatrixType::Scalar Scalar;
typedef typename ei_nested<MatrixType>::type MatrixTypeNested;
typedef typename ei_unref<MatrixTypeNested>::type _MatrixTypeNested;
enum {
RowsAtCompileTime = MatrixType::ColsAtCompileTime,
ColsAtCompileTime = MatrixType::RowsAtCompileTime,
MaxRowsAtCompileTime = MatrixType::MaxColsAtCompileTime,
MaxColsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
Flags = ((int(_MatrixTypeNested::Flags) ^ RowMajorBit)
& ~(LowerTriangularBit | UpperTriangularBit))
| (int(_MatrixTypeNested::Flags)&UpperTriangularBit ? LowerTriangularBit : 0)
| (int(_MatrixTypeNested::Flags)&LowerTriangularBit ? UpperTriangularBit : 0),
CoeffReadCost = _MatrixTypeNested::CoeffReadCost
};
};
template<typename MatrixType> class Transpose
: public MatrixBase<Transpose<MatrixType> >
{
public:
EIGEN_GENERIC_PUBLIC_INTERFACE(Transpose)
inline Transpose(const MatrixType& matrix) : m_matrix(matrix) {}
EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Transpose)
inline int rows() const { return m_matrix.cols(); }
inline int cols() const { return m_matrix.rows(); }
inline int stride(void) const { return m_matrix.stride(); }
inline Scalar& coeffRef(int row, int col)
{
return m_matrix.const_cast_derived().coeffRef(col, row);
}
inline const Scalar coeff(int row, int col) const
{
return m_matrix.coeff(col, row);
}
inline const Scalar coeff(int index) const
{
return m_matrix.coeff(index);
}
inline Scalar& coeffRef(int index)
{
return m_matrix.const_cast_derived().coeffRef(index);
}
template<int LoadMode>
inline const PacketScalar packet(int row, int col) const
{
return m_matrix.template packet<LoadMode>(col, row);
}
template<int LoadMode>
inline void writePacket(int row, int col, const PacketScalar& x)
{
m_matrix.const_cast_derived().template writePacket<LoadMode>(col, row, x);
}
template<int LoadMode>
inline const PacketScalar packet(int index) const
{
return m_matrix.template packet<LoadMode>(index);
}
template<int LoadMode>
inline void writePacket(int index, const PacketScalar& x)
{
m_matrix.const_cast_derived().template writePacket<LoadMode>(index, x);
}
protected:
const typename MatrixType::Nested m_matrix;
};
/** \returns an expression of the transpose of *this.
*
* Example: \include MatrixBase_transpose.cpp
* Output: \verbinclude MatrixBase_transpose.out
*
* \warning If you want to replace a matrix by its own transpose, do \b NOT do this:
* \code
* m = m.transpose(); // bug!!! caused by aliasing effect
* \endcode
* Instead, use the transposeInPlace() method:
* \code
* m.transposeInPlace();
* \endcode
* which gives Eigen good opportunities for optimization, or alternatively you can also do:
* \code
* m = m.transpose().eval();
* \endcode
*
* \sa transposeInPlace(), adjoint() */
template<typename Derived>
inline Transpose<Derived>
MatrixBase<Derived>::transpose()
{
return derived();
}
/** This is the const version of transpose().
*
* Make sure you read the warning for transpose() !
*
* \sa transposeInPlace(), adjoint() */
template<typename Derived>
inline const Transpose<Derived>
MatrixBase<Derived>::transpose() const
{
return derived();
}
/** \returns an expression of the adjoint (i.e. conjugate transpose) of *this.
*
* Example: \include MatrixBase_adjoint.cpp
* Output: \verbinclude MatrixBase_adjoint.out
*
* \warning If you want to replace a matrix by its own adjoint, do \b NOT do this:
* \code
* m = m.adjoint(); // bug!!! caused by aliasing effect
* \endcode
* Instead, do:
* \code
* m = m.adjoint().eval();
* \endcode
*
* \sa transpose(), conjugate(), class Transpose, class ei_scalar_conjugate_op */
template<typename Derived>
inline const typename MatrixBase<Derived>::AdjointReturnType
MatrixBase<Derived>::adjoint() const
{
return conjugate().nestByValue();
}
/***************************************************************************
* "in place" transpose implementation
***************************************************************************/
template<typename MatrixType,
bool IsSquare = (MatrixType::RowsAtCompileTime == MatrixType::ColsAtCompileTime) && MatrixType::RowsAtCompileTime!=Dynamic>
struct ei_inplace_transpose_selector;
template<typename MatrixType>
struct ei_inplace_transpose_selector<MatrixType,true> { // square matrix
static void run(MatrixType& m) {
m.template part<StrictlyUpperTriangular>().swap(m.transpose());
}
};
template<typename MatrixType>
struct ei_inplace_transpose_selector<MatrixType,false> { // non square matrix
static void run(MatrixType& m) {
if (m.rows()==m.cols())
m.template part<StrictlyUpperTriangular>().swap(m.transpose());
else
m = m.transpose().eval();
}
};
/** This is the "in place" version of transpose: it transposes \c *this.
*
* In most cases it is probably better to simply use the transposed expression
* of a matrix. However, when transposing the matrix data itself is really needed,
* then this "in-place" version is probably the right choice because it provides
* the following additional features:
* - less error prone: doing the same operation with .transpose() requires special care:
* \code m = m.transpose().eval(); \endcode
* - no temporary object is created (currently only for squared matrices)
* - it allows future optimizations (cache friendliness, etc.)
*
* \note if the matrix is not square, then \c *this must be a resizable matrix.
*
* \sa transpose(), adjoint() */
template<typename Derived>
inline void MatrixBase<Derived>::transposeInPlace()
{
ei_inplace_transpose_selector<Derived>::run(derived());
}
#endif // EIGEN_TRANSPOSE_H

228
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/Visitor.h Normal file
View File

@ -0,0 +1,228 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_VISITOR_H
#define EIGEN_VISITOR_H
template<typename Visitor, typename Derived, int UnrollCount>
struct ei_visitor_impl
{
enum {
col = (UnrollCount-1) / Derived::RowsAtCompileTime,
row = (UnrollCount-1) % Derived::RowsAtCompileTime
};
inline static void run(const Derived &mat, Visitor& visitor)
{
ei_visitor_impl<Visitor, Derived, UnrollCount-1>::run(mat, visitor);
visitor(mat.coeff(row, col), row, col);
}
};
template<typename Visitor, typename Derived>
struct ei_visitor_impl<Visitor, Derived, 1>
{
inline static void run(const Derived &mat, Visitor& visitor)
{
return visitor.init(mat.coeff(0, 0), 0, 0);
}
};
template<typename Visitor, typename Derived>
struct ei_visitor_impl<Visitor, Derived, Dynamic>
{
inline static void run(const Derived& mat, Visitor& visitor)
{
visitor.init(mat.coeff(0,0), 0, 0);
for(int i = 1; i < mat.rows(); ++i)
visitor(mat.coeff(i, 0), i, 0);
for(int j = 1; j < mat.cols(); ++j)
for(int i = 0; i < mat.rows(); ++i)
visitor(mat.coeff(i, j), i, j);
}
};
/** Applies the visitor \a visitor to the whole coefficients of the matrix or vector.
*
* The template parameter \a Visitor is the type of the visitor and provides the following interface:
* \code
* struct MyVisitor {
* // called for the first coefficient
* void init(const Scalar& value, int i, int j);
* // called for all other coefficients
* void operator() (const Scalar& value, int i, int j);
* };
* \endcode
*
* \note compared to one or two \em for \em loops, visitors offer automatic
* unrolling for small fixed size matrix.
*
* \sa minCoeff(int*,int*), maxCoeff(int*,int*), MatrixBase::redux()
*/
template<typename Derived>
template<typename Visitor>
void MatrixBase<Derived>::visit(Visitor& visitor) const
{
const bool unroll = SizeAtCompileTime * CoeffReadCost
+ (SizeAtCompileTime-1) * ei_functor_traits<Visitor>::Cost
<= EIGEN_UNROLLING_LIMIT;
return ei_visitor_impl<Visitor, Derived,
unroll ? int(SizeAtCompileTime) : Dynamic
>::run(derived(), visitor);
}
/** \internal
* \brief Base class to implement min and max visitors
*/
template <typename Scalar>
struct ei_coeff_visitor
{
int row, col;
Scalar res;
inline void init(const Scalar& value, int i, int j)
{
res = value;
row = i;
col = j;
}
};
/** \internal
* \brief Visitor computing the min coefficient with its value and coordinates
*
* \sa MatrixBase::minCoeff(int*, int*)
*/
template <typename Scalar>
struct ei_min_coeff_visitor : ei_coeff_visitor<Scalar>
{
void operator() (const Scalar& value, int i, int j)
{
if(value < this->res)
{
this->res = value;
this->row = i;
this->col = j;
}
}
};
template<typename Scalar>
struct ei_functor_traits<ei_min_coeff_visitor<Scalar> > {
enum {
Cost = NumTraits<Scalar>::AddCost
};
};
/** \internal
* \brief Visitor computing the max coefficient with its value and coordinates
*
* \sa MatrixBase::maxCoeff(int*, int*)
*/
template <typename Scalar>
struct ei_max_coeff_visitor : ei_coeff_visitor<Scalar>
{
void operator() (const Scalar& value, int i, int j)
{
if(value > this->res)
{
this->res = value;
this->row = i;
this->col = j;
}
}
};
template<typename Scalar>
struct ei_functor_traits<ei_max_coeff_visitor<Scalar> > {
enum {
Cost = NumTraits<Scalar>::AddCost
};
};
/** \returns the minimum of all coefficients of *this
* and puts in *row and *col its location.
*
* \sa MatrixBase::minCoeff(int*), MatrixBase::maxCoeff(int*,int*), MatrixBase::visitor(), MatrixBase::minCoeff()
*/
template<typename Derived>
typename ei_traits<Derived>::Scalar
MatrixBase<Derived>::minCoeff(int* row, int* col) const
{
ei_min_coeff_visitor<Scalar> minVisitor;
this->visit(minVisitor);
*row = minVisitor.row;
if (col) *col = minVisitor.col;
return minVisitor.res;
}
/** \returns the minimum of all coefficients of *this
* and puts in *index its location.
*
* \sa MatrixBase::minCoeff(int*,int*), MatrixBase::maxCoeff(int*,int*), MatrixBase::visitor(), MatrixBase::minCoeff()
*/
template<typename Derived>
typename ei_traits<Derived>::Scalar
MatrixBase<Derived>::minCoeff(int* index) const
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
ei_min_coeff_visitor<Scalar> minVisitor;
this->visit(minVisitor);
*index = (RowsAtCompileTime==1) ? minVisitor.col : minVisitor.row;
return minVisitor.res;
}
/** \returns the maximum of all coefficients of *this
* and puts in *row and *col its location.
*
* \sa MatrixBase::minCoeff(int*,int*), MatrixBase::visitor(), MatrixBase::maxCoeff()
*/
template<typename Derived>
typename ei_traits<Derived>::Scalar
MatrixBase<Derived>::maxCoeff(int* row, int* col) const
{
ei_max_coeff_visitor<Scalar> maxVisitor;
this->visit(maxVisitor);
*row = maxVisitor.row;
if (col) *col = maxVisitor.col;
return maxVisitor.res;
}
/** \returns the maximum of all coefficients of *this
* and puts in *index its location.
*
* \sa MatrixBase::maxCoeff(int*,int*), MatrixBase::minCoeff(int*,int*), MatrixBase::visitor(), MatrixBase::maxCoeff()
*/
template<typename Derived>
typename ei_traits<Derived>::Scalar
MatrixBase<Derived>::maxCoeff(int* index) const
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
ei_max_coeff_visitor<Scalar> maxVisitor;
this->visit(maxVisitor);
*index = (RowsAtCompileTime==1) ? maxVisitor.col : maxVisitor.row;
return maxVisitor.res;
}
#endif // EIGEN_VISITOR_H

6
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/arch/AltiVec/CMakeLists.txt Normal file
View File

@ -0,0 +1,6 @@
FILE(GLOB Eigen_Core_arch_AltiVec_SRCS "*.h")
INSTALL(FILES
${Eigen_Core_arch_AltiVec_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Core/arch/AltiVec
)

354
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/arch/AltiVec/PacketMath.h Normal file
View File

@ -0,0 +1,354 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Konstantinos Margaritis <markos@codex.gr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_PACKET_MATH_ALTIVEC_H
#define EIGEN_PACKET_MATH_ALTIVEC_H
#ifndef EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD
#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 4
#endif
typedef __vector float v4f;
typedef __vector int v4i;
typedef __vector unsigned int v4ui;
typedef __vector __bool int v4bi;
// We don't want to write the same code all the time, but we need to reuse the constants
// and it doesn't really work to declare them global, so we define macros instead
#define USE_CONST_v0i const v4i v0i = vec_splat_s32(0)
#define USE_CONST_v1i const v4i v1i = vec_splat_s32(1)
#define USE_CONST_v16i_ const v4i v16i_ = vec_splat_s32(-16)
#define USE_CONST_v0f USE_CONST_v0i; const v4f v0f = (v4f) v0i
#define USE_CONST_v1f USE_CONST_v1i; const v4f v1f = vec_ctf(v1i, 0)
#define USE_CONST_v1i_ const v4ui v1i_ = vec_splat_u32(-1)
#define USE_CONST_v0f_ USE_CONST_v1i_; const v4f v0f_ = (v4f) vec_sl(v1i_, v1i_)
template<> struct ei_packet_traits<float> { typedef v4f type; enum {size=4}; };
template<> struct ei_packet_traits<int> { typedef v4i type; enum {size=4}; };
template<> struct ei_unpacket_traits<v4f> { typedef float type; enum {size=4}; };
template<> struct ei_unpacket_traits<v4i> { typedef int type; enum {size=4}; };
inline std::ostream & operator <<(std::ostream & s, const v4f & v)
{
union {
v4f v;
float n[4];
} vt;
vt.v = v;
s << vt.n[0] << ", " << vt.n[1] << ", " << vt.n[2] << ", " << vt.n[3];
return s;
}
inline std::ostream & operator <<(std::ostream & s, const v4i & v)
{
union {
v4i v;
int n[4];
} vt;
vt.v = v;
s << vt.n[0] << ", " << vt.n[1] << ", " << vt.n[2] << ", " << vt.n[3];
return s;
}
inline std::ostream & operator <<(std::ostream & s, const v4ui & v)
{
union {
v4ui v;
unsigned int n[4];
} vt;
vt.v = v;
s << vt.n[0] << ", " << vt.n[1] << ", " << vt.n[2] << ", " << vt.n[3];
return s;
}
inline std::ostream & operator <<(std::ostream & s, const v4bi & v)
{
union {
__vector __bool int v;
unsigned int n[4];
} vt;
vt.v = v;
s << vt.n[0] << ", " << vt.n[1] << ", " << vt.n[2] << ", " << vt.n[3];
return s;
}
template<> inline v4f ei_padd(const v4f& a, const v4f& b) { return vec_add(a,b); }
template<> inline v4i ei_padd(const v4i& a, const v4i& b) { return vec_add(a,b); }
template<> inline v4f ei_psub(const v4f& a, const v4f& b) { return vec_sub(a,b); }
template<> inline v4i ei_psub(const v4i& a, const v4i& b) { return vec_sub(a,b); }
template<> inline v4f ei_pmul(const v4f& a, const v4f& b) { USE_CONST_v0f; return vec_madd(a,b, v0f); }
template<> inline v4i ei_pmul(const v4i& a, const v4i& b)
{
// Detailed in: http://freevec.org/content/32bit_signed_integer_multiplication_altivec
//Set up constants, variables
v4i a1, b1, bswap, low_prod, high_prod, prod, prod_, v1sel;
USE_CONST_v0i;
USE_CONST_v1i;
USE_CONST_v16i_;
// Get the absolute values
a1 = vec_abs(a);
b1 = vec_abs(b);
// Get the signs using xor
v4bi sgn = (v4bi) vec_cmplt(vec_xor(a, b), v0i);
// Do the multiplication for the asbolute values.
bswap = (v4i) vec_rl((v4ui) b1, (v4ui) v16i_ );
low_prod = vec_mulo((__vector short)a1, (__vector short)b1);
high_prod = vec_msum((__vector short)a1, (__vector short)bswap, v0i);
high_prod = (v4i) vec_sl((v4ui) high_prod, (v4ui) v16i_);
prod = vec_add( low_prod, high_prod );
// NOR the product and select only the negative elements according to the sign mask
prod_ = vec_nor(prod, prod);
prod_ = vec_sel(v0i, prod_, sgn);
// Add 1 to the result to get the negative numbers
v1sel = vec_sel(v0i, v1i, sgn);
prod_ = vec_add(prod_, v1sel);
// Merge the results back to the final vector.
prod = vec_sel(prod, prod_, sgn);
return prod;
}
template<> inline v4f ei_pdiv(const v4f& a, const v4f& b) {
v4f t, y_0, y_1, res;
USE_CONST_v0f;
USE_CONST_v1f;
// Altivec does not offer a divide instruction, we have to do a reciprocal approximation
y_0 = vec_re(b);
// Do one Newton-Raphson iteration to get the needed accuracy
t = vec_nmsub(y_0, b, v1f);
y_1 = vec_madd(y_0, t, y_0);
res = vec_madd(a, y_1, v0f);
return res;
}
template<> inline v4f ei_pmadd(const v4f& a, const v4f& b, const v4f& c) { return vec_madd(a, b, c); }
template<> inline v4f ei_pmin(const v4f& a, const v4f& b) { return vec_min(a,b); }
template<> inline v4i ei_pmin(const v4i& a, const v4i& b) { return vec_min(a,b); }
template<> inline v4f ei_pmax(const v4f& a, const v4f& b) { return vec_max(a,b); }
template<> inline v4i ei_pmax(const v4i& a, const v4i& b) { return vec_max(a,b); }
template<> inline v4f ei_pload(const float* from) { return vec_ld(0, from); }
template<> inline v4i ei_pload(const int* from) { return vec_ld(0, from); }
template<> inline v4f ei_ploadu(const float* from)
{
// Taken from http://developer.apple.com/hardwaredrivers/ve/alignment.html
__vector unsigned char MSQ, LSQ;
__vector unsigned char mask;
MSQ = vec_ld(0, (unsigned char *)from); // most significant quadword
LSQ = vec_ld(15, (unsigned char *)from); // least significant quadword
mask = vec_lvsl(0, from); // create the permute mask
return (v4f) vec_perm(MSQ, LSQ, mask); // align the data
}
template<> inline v4i ei_ploadu(const int* from)
{
// Taken from http://developer.apple.com/hardwaredrivers/ve/alignment.html
__vector unsigned char MSQ, LSQ;
__vector unsigned char mask;
MSQ = vec_ld(0, (unsigned char *)from); // most significant quadword
LSQ = vec_ld(15, (unsigned char *)from); // least significant quadword
mask = vec_lvsl(0, from); // create the permute mask
return (v4i) vec_perm(MSQ, LSQ, mask); // align the data
}
template<> inline v4f ei_pset1(const float& from)
{
// Taken from http://developer.apple.com/hardwaredrivers/ve/alignment.html
float __attribute__(aligned(16)) af[4];
af[0] = from;
v4f vc = vec_ld(0, af);
vc = vec_splat(vc, 0);
return vc;
}
template<> inline v4i ei_pset1(const int& from)
{
int __attribute__(aligned(16)) ai[4];
ai[0] = from;
v4i vc = vec_ld(0, ai);
vc = vec_splat(vc, 0);
return vc;
}
template<> inline void ei_pstore(float* to, const v4f& from) { vec_st(from, 0, to); }
template<> inline void ei_pstore(int* to, const v4i& from) { vec_st(from, 0, to); }
template<> inline void ei_pstoreu(float* to, const v4f& from)
{
// Taken from http://developer.apple.com/hardwaredrivers/ve/alignment.html
// Warning: not thread safe!
__vector unsigned char MSQ, LSQ, edges;
__vector unsigned char edgeAlign, align;
MSQ = vec_ld(0, (unsigned char *)to); // most significant quadword
LSQ = vec_ld(15, (unsigned char *)to); // least significant quadword
edgeAlign = vec_lvsl(0, to); // permute map to extract edges
edges=vec_perm(LSQ,MSQ,edgeAlign); // extract the edges
align = vec_lvsr( 0, to ); // permute map to misalign data
MSQ = vec_perm(edges,(__vector unsigned char)from,align); // misalign the data (MSQ)
LSQ = vec_perm((__vector unsigned char)from,edges,align); // misalign the data (LSQ)
vec_st( LSQ, 15, (unsigned char *)to ); // Store the LSQ part first
vec_st( MSQ, 0, (unsigned char *)to ); // Store the MSQ part
}
template<> inline void ei_pstoreu(int* to , const v4i& from )
{
// Taken from http://developer.apple.com/hardwaredrivers/ve/alignment.html
// Warning: not thread safe!
__vector unsigned char MSQ, LSQ, edges;
__vector unsigned char edgeAlign, align;
MSQ = vec_ld(0, (unsigned char *)to); // most significant quadword
LSQ = vec_ld(15, (unsigned char *)to); // least significant quadword
edgeAlign = vec_lvsl(0, to); // permute map to extract edges
edges=vec_perm(LSQ,MSQ,edgeAlign); // extract the edges
align = vec_lvsr( 0, to ); // permute map to misalign data
MSQ = vec_perm(edges,(__vector unsigned char)from,align); // misalign the data (MSQ)
LSQ = vec_perm((__vector unsigned char)from,edges,align); // misalign the data (LSQ)
vec_st( LSQ, 15, (unsigned char *)to ); // Store the LSQ part first
vec_st( MSQ, 0, (unsigned char *)to ); // Store the MSQ part
}
template<> inline float ei_pfirst(const v4f& a)
{
float __attribute__(aligned(16)) af[4];
vec_st(a, 0, af);
return af[0];
}
template<> inline int ei_pfirst(const v4i& a)
{
int __attribute__(aligned(16)) ai[4];
vec_st(a, 0, ai);
return ai[0];
}
inline v4f ei_preduxp(const v4f* vecs)
{
v4f v[4], sum[4];
// It's easier and faster to transpose then add as columns
// Check: http://www.freevec.org/function/matrix_4x4_transpose_floats for explanation
// Do the transpose, first set of moves
v[0] = vec_mergeh(vecs[0], vecs[2]);
v[1] = vec_mergel(vecs[0], vecs[2]);
v[2] = vec_mergeh(vecs[1], vecs[3]);
v[3] = vec_mergel(vecs[1], vecs[3]);
// Get the resulting vectors
sum[0] = vec_mergeh(v[0], v[2]);
sum[1] = vec_mergel(v[0], v[2]);
sum[2] = vec_mergeh(v[1], v[3]);
sum[3] = vec_mergel(v[1], v[3]);
// Now do the summation:
// Lines 0+1
sum[0] = vec_add(sum[0], sum[1]);
// Lines 2+3
sum[1] = vec_add(sum[2], sum[3]);
// Add the results
sum[0] = vec_add(sum[0], sum[1]);
return sum[0];
}
inline float ei_predux(const v4f& a)
{
v4f b, sum;
b = (v4f)vec_sld(a, a, 8);
sum = vec_add(a, b);
b = (v4f)vec_sld(sum, sum, 4);
sum = vec_add(sum, b);
return ei_pfirst(sum);
}
inline v4i ei_preduxp(const v4i* vecs)
{
v4i v[4], sum[4];
// It's easier and faster to transpose then add as columns
// Check: http://www.freevec.org/function/matrix_4x4_transpose_floats for explanation
// Do the transpose, first set of moves
v[0] = vec_mergeh(vecs[0], vecs[2]);
v[1] = vec_mergel(vecs[0], vecs[2]);
v[2] = vec_mergeh(vecs[1], vecs[3]);
v[3] = vec_mergel(vecs[1], vecs[3]);
// Get the resulting vectors
sum[0] = vec_mergeh(v[0], v[2]);
sum[1] = vec_mergel(v[0], v[2]);
sum[2] = vec_mergeh(v[1], v[3]);
sum[3] = vec_mergel(v[1], v[3]);
// Now do the summation:
// Lines 0+1
sum[0] = vec_add(sum[0], sum[1]);
// Lines 2+3
sum[1] = vec_add(sum[2], sum[3]);
// Add the results
sum[0] = vec_add(sum[0], sum[1]);
return sum[0];
}
inline int ei_predux(const v4i& a)
{
USE_CONST_v0i;
v4i sum;
sum = vec_sums(a, v0i);
sum = vec_sld(sum, v0i, 12);
return ei_pfirst(sum);
}
template<int Offset>
struct ei_palign_impl<Offset, v4f>
{
inline static void run(v4f& first, const v4f& second)
{
first = vec_sld(first, second, Offset*4);
}
};
template<int Offset>
struct ei_palign_impl<Offset, v4i>
{
inline static void run(v4i& first, const v4i& second)
{
first = vec_sld(first, second, Offset*4);
}
};
#endif // EIGEN_PACKET_MATH_ALTIVEC_H

6
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/arch/SSE/CMakeLists.txt Normal file
View File

@ -0,0 +1,6 @@
FILE(GLOB Eigen_Core_arch_SSE_SRCS "*.h")
INSTALL(FILES
${Eigen_Core_arch_SSE_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Core/arch/SSE
)

327
ground/openpilotgcs/src/libs/eigen/Eigen/src/Core/arch/SSE/PacketMath.h Normal file
View File

@ -0,0 +1,327 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_PACKET_MATH_SSE_H
#define EIGEN_PACKET_MATH_SSE_H
#ifndef EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD
#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 16
#endif
template<> struct ei_packet_traits<float> { typedef __m128 type; enum {size=4}; };
template<> struct ei_packet_traits<double> { typedef __m128d type; enum {size=2}; };
template<> struct ei_packet_traits<int> { typedef __m128i type; enum {size=4}; };
template<> struct ei_unpacket_traits<__m128> { typedef float type; enum {size=4}; };
template<> struct ei_unpacket_traits<__m128d> { typedef double type; enum {size=2}; };
template<> struct ei_unpacket_traits<__m128i> { typedef int type; enum {size=4}; };
template<> EIGEN_STRONG_INLINE __m128 ei_pset1<float>(const float& from) { return _mm_set1_ps(from); }
template<> EIGEN_STRONG_INLINE __m128d ei_pset1<double>(const double& from) { return _mm_set1_pd(from); }
template<> EIGEN_STRONG_INLINE __m128i ei_pset1<int>(const int& from) { return _mm_set1_epi32(from); }
template<> EIGEN_STRONG_INLINE __m128 ei_padd<__m128>(const __m128& a, const __m128& b) { return _mm_add_ps(a,b); }
template<> EIGEN_STRONG_INLINE __m128d ei_padd<__m128d>(const __m128d& a, const __m128d& b) { return _mm_add_pd(a,b); }
template<> EIGEN_STRONG_INLINE __m128i ei_padd<__m128i>(const __m128i& a, const __m128i& b) { return _mm_add_epi32(a,b); }
template<> EIGEN_STRONG_INLINE __m128 ei_psub<__m128>(const __m128& a, const __m128& b) { return _mm_sub_ps(a,b); }
template<> EIGEN_STRONG_INLINE __m128d ei_psub<__m128d>(const __m128d& a, const __m128d& b) { return _mm_sub_pd(a,b); }
template<> EIGEN_STRONG_INLINE __m128i ei_psub<__m128i>(const __m128i& a, const __m128i& b) { return _mm_sub_epi32(a,b); }
template<> EIGEN_STRONG_INLINE __m128 ei_pmul<__m128>(const __m128& a, const __m128& b) { return _mm_mul_ps(a,b); }
template<> EIGEN_STRONG_INLINE __m128d ei_pmul<__m128d>(const __m128d& a, const __m128d& b) { return _mm_mul_pd(a,b); }
template<> EIGEN_STRONG_INLINE __m128i ei_pmul<__m128i>(const __m128i& a, const __m128i& b)
{
return _mm_or_si128(
_mm_and_si128(
_mm_mul_epu32(a,b),
_mm_setr_epi32(0xffffffff,0,0xffffffff,0)),
_mm_slli_si128(
_mm_and_si128(
_mm_mul_epu32(_mm_srli_si128(a,4),_mm_srli_si128(b,4)),
_mm_setr_epi32(0xffffffff,0,0xffffffff,0)), 4));
}
template<> EIGEN_STRONG_INLINE __m128 ei_pdiv<__m128>(const __m128& a, const __m128& b) { return _mm_div_ps(a,b); }
template<> EIGEN_STRONG_INLINE __m128d ei_pdiv<__m128d>(const __m128d& a, const __m128d& b) { return _mm_div_pd(a,b); }
template<> EIGEN_STRONG_INLINE __m128i ei_pdiv<__m128i>(const __m128i& /*a*/, const __m128i& /*b*/)
{ ei_assert(false && "packet integer division are not supported by SSE");
__m128i dummy = ei_pset1<int>(0);
return dummy;
}
// for some weird raisons, it has to be overloaded for packet integer
template<> EIGEN_STRONG_INLINE __m128i ei_pmadd(const __m128i& a, const __m128i& b, const __m128i& c) { return ei_padd(ei_pmul(a,b), c); }
template<> EIGEN_STRONG_INLINE __m128 ei_pmin<__m128>(const __m128& a, const __m128& b) { return _mm_min_ps(a,b); }
template<> EIGEN_STRONG_INLINE __m128d ei_pmin<__m128d>(const __m128d& a, const __m128d& b) { return _mm_min_pd(a,b); }
// FIXME this vectorized min operator is likely to be slower than the standard one
template<> EIGEN_STRONG_INLINE __m128i ei_pmin<__m128i>(const __m128i& a, const __m128i& b)
{
__m128i mask = _mm_cmplt_epi32(a,b);
return _mm_or_si128(_mm_and_si128(mask,a),_mm_andnot_si128(mask,b));
}
template<> EIGEN_STRONG_INLINE __m128 ei_pmax<__m128>(const __m128& a, const __m128& b) { return _mm_max_ps(a,b); }
template<> EIGEN_STRONG_INLINE __m128d ei_pmax<__m128d>(const __m128d& a, const __m128d& b) { return _mm_max_pd(a,b); }
// FIXME this vectorized max operator is likely to be slower than the standard one
template<> EIGEN_STRONG_INLINE __m128i ei_pmax<__m128i>(const __m128i& a, const __m128i& b)
{
__m128i mask = _mm_cmpgt_epi32(a,b);
return _mm_or_si128(_mm_and_si128(mask,a),_mm_andnot_si128(mask,b));
}
template<> EIGEN_STRONG_INLINE __m128 ei_pload<float>(const float* from) { return _mm_load_ps(from); }
template<> EIGEN_STRONG_INLINE __m128d ei_pload<double>(const double* from) { return _mm_load_pd(from); }
template<> EIGEN_STRONG_INLINE __m128i ei_pload<int>(const int* from) { return _mm_load_si128(reinterpret_cast<const __m128i*>(from)); }
template<> EIGEN_STRONG_INLINE __m128 ei_ploadu<float>(const float* from) { return _mm_loadu_ps(from); }
// template<> EIGEN_STRONG_INLINE __m128 ei_ploadu(const float* from) {
// if (size_t(from)&0xF)
// return _mm_loadu_ps(from);
// else
// return _mm_loadu_ps(from);
// }
template<> EIGEN_STRONG_INLINE __m128d ei_ploadu<double>(const double* from) { return _mm_loadu_pd(from); }
template<> EIGEN_STRONG_INLINE __m128i ei_ploadu<int>(const int* from) { return _mm_loadu_si128(reinterpret_cast<const __m128i*>(from)); }
template<> EIGEN_STRONG_INLINE void ei_pstore<float>(float* to, const __m128& from) { _mm_store_ps(to, from); }
template<> EIGEN_STRONG_INLINE void ei_pstore<double>(double* to, const __m128d& from) { _mm_store_pd(to, from); }
template<> EIGEN_STRONG_INLINE void ei_pstore<int>(int* to, const __m128i& from) { _mm_store_si128(reinterpret_cast<__m128i*>(to), from); }
template<> EIGEN_STRONG_INLINE void ei_pstoreu<float>(float* to, const __m128& from) { _mm_storeu_ps(to, from); }
template<> EIGEN_STRONG_INLINE void ei_pstoreu<double>(double* to, const __m128d& from) { _mm_storeu_pd(to, from); }
template<> EIGEN_STRONG_INLINE void ei_pstoreu<int>(int* to, const __m128i& from) { _mm_storeu_si128(reinterpret_cast<__m128i*>(to), from); }
#if defined(_MSC_VER) && (_MSC_VER <= 1500) && defined(_WIN64) && !defined(__INTEL_COMPILER)
// The temporary variable fixes an internal compilation error.
// Direct of the struct members fixed bug #62.
template<> EIGEN_STRONG_INLINE float ei_pfirst<__m128>(const __m128& a) { return a.m128_f32[0]; }
template<> EIGEN_STRONG_INLINE double ei_pfirst<__m128d>(const __m128d& a) { return a.m128d_f64[0]; }
template<> EIGEN_STRONG_INLINE int ei_pfirst<__m128i>(const __m128i& a) { int x = _mm_cvtsi128_si32(a); return x; }
#elif defined(_MSC_VER) && (_MSC_VER <= 1500) && !defined(__INTEL_COMPILER)
// The temporary variable fixes an internal compilation error.
template<> EIGEN_STRONG_INLINE float ei_pfirst<__m128>(const __m128& a) { float x = _mm_cvtss_f32(a); return x; }
template<> EIGEN_STRONG_INLINE double ei_pfirst<__m128d>(const __m128d& a) { double x = _mm_cvtsd_f64(a); return x; }
template<> EIGEN_STRONG_INLINE int ei_pfirst<__m128i>(const __m128i& a) { int x = _mm_cvtsi128_si32(a); return x; }
#else
template<> EIGEN_STRONG_INLINE float ei_pfirst<__m128>(const __m128& a) { return _mm_cvtss_f32(a); }
template<> EIGEN_STRONG_INLINE double ei_pfirst<__m128d>(const __m128d& a) { return _mm_cvtsd_f64(a); }
template<> EIGEN_STRONG_INLINE int ei_pfirst<__m128i>(const __m128i& a) { return _mm_cvtsi128_si32(a); }
#endif
#ifdef __SSE3__
// TODO implement SSE2 versions as well as integer versions
template<> EIGEN_STRONG_INLINE __m128 ei_preduxp<__m128>(const __m128* vecs)
{
return _mm_hadd_ps(_mm_hadd_ps(vecs[0], vecs[1]),_mm_hadd_ps(vecs[2], vecs[3]));
}
template<> EIGEN_STRONG_INLINE __m128d ei_preduxp<__m128d>(const __m128d* vecs)
{
return _mm_hadd_pd(vecs[0], vecs[1]);
}
// SSSE3 version:
// EIGEN_STRONG_INLINE __m128i ei_preduxp(const __m128i* vecs)
// {
// return _mm_hadd_epi32(_mm_hadd_epi32(vecs[0], vecs[1]),_mm_hadd_epi32(vecs[2], vecs[3]));
// }
template<> EIGEN_STRONG_INLINE float ei_predux<__m128>(const __m128& a)
{
__m128 tmp0 = _mm_hadd_ps(a,a);
return ei_pfirst(_mm_hadd_ps(tmp0, tmp0));
}
template<> EIGEN_STRONG_INLINE double ei_predux<__m128d>(const __m128d& a) { return ei_pfirst(_mm_hadd_pd(a, a)); }
// SSSE3 version:
// EIGEN_STRONG_INLINE float ei_predux(const __m128i& a)
// {
// __m128i tmp0 = _mm_hadd_epi32(a,a);
// return ei_pfirst(_mm_hadd_epi32(tmp0, tmp0));
// }
#else
// SSE2 versions
template<> EIGEN_STRONG_INLINE float ei_predux<__m128>(const __m128& a)
{
__m128 tmp = _mm_add_ps(a, _mm_movehl_ps(a,a));
return ei_pfirst(_mm_add_ss(tmp, _mm_shuffle_ps(tmp,tmp, 1)));
}
template<> EIGEN_STRONG_INLINE double ei_predux<__m128d>(const __m128d& a)
{
return ei_pfirst(_mm_add_sd(a, _mm_unpackhi_pd(a,a)));
}
template<> EIGEN_STRONG_INLINE __m128 ei_preduxp<__m128>(const __m128* vecs)
{
__m128 tmp0, tmp1, tmp2;
tmp0 = _mm_unpacklo_ps(vecs[0], vecs[1]);
tmp1 = _mm_unpackhi_ps(vecs[0], vecs[1]);
tmp2 = _mm_unpackhi_ps(vecs[2], vecs[3]);
tmp0 = _mm_add_ps(tmp0, tmp1);
tmp1 = _mm_unpacklo_ps(vecs[2], vecs[3]);
tmp1 = _mm_add_ps(tmp1, tmp2);
tmp2 = _mm_movehl_ps(tmp1, tmp0);
tmp0 = _mm_movelh_ps(tmp0, tmp1);
return _mm_add_ps(tmp0, tmp2);
}
template<> EIGEN_STRONG_INLINE __m128d ei_preduxp<__m128d>(const __m128d* vecs)
{
return _mm_add_pd(_mm_unpacklo_pd(vecs[0], vecs[1]), _mm_unpackhi_pd(vecs[0], vecs[1]));
}
#endif // SSE3
template<> EIGEN_STRONG_INLINE int ei_predux<__m128i>(const __m128i& a)
{
__m128i tmp = _mm_add_epi32(a, _mm_unpackhi_epi64(a,a));
return ei_pfirst(tmp) + ei_pfirst(_mm_shuffle_epi32(tmp, 1));
}
template<> EIGEN_STRONG_INLINE __m128i ei_preduxp<__m128i>(const __m128i* vecs)
{
__m128i tmp0, tmp1, tmp2;
tmp0 = _mm_unpacklo_epi32(vecs[0], vecs[1]);
tmp1 = _mm_unpackhi_epi32(vecs[0], vecs[1]);
tmp2 = _mm_unpackhi_epi32(vecs[2], vecs[3]);
tmp0 = _mm_add_epi32(tmp0, tmp1);
tmp1 = _mm_unpacklo_epi32(vecs[2], vecs[3]);
tmp1 = _mm_add_epi32(tmp1, tmp2);
tmp2 = _mm_unpacklo_epi64(tmp0, tmp1);
tmp0 = _mm_unpackhi_epi64(tmp0, tmp1);
return _mm_add_epi32(tmp0, tmp2);
}
#if (defined __GNUC__)
// template <> EIGEN_STRONG_INLINE __m128 ei_pmadd(const __m128& a, const __m128& b, const __m128& c)
// {
// __m128 res = b;
// asm("mulps %[a], %[b] \n\taddps %[c], %[b]" : [b] "+x" (res) : [a] "x" (a), [c] "x" (c));
// return res;
// }
// EIGEN_STRONG_INLINE __m128i _mm_alignr_epi8(const __m128i& a, const __m128i& b, const int i)
// {
// __m128i res = a;
// asm("palignr %[i], %[a], %[b] " : [b] "+x" (res) : [a] "x" (a), [i] "i" (i));
// return res;
// }
#endif
#ifdef __SSSE3__
// SSSE3 versions
template<int Offset>
struct ei_palign_impl<Offset,__m128>
{
EIGEN_STRONG_INLINE static void run(__m128& first, const __m128& second)
{
if (Offset!=0)
first = _mm_castsi128_ps(_mm_alignr_epi8(_mm_castps_si128(second), _mm_castps_si128(first), Offset*4));
}
};
template<int Offset>
struct ei_palign_impl<Offset,__m128i>
{
EIGEN_STRONG_INLINE static void run(__m128i& first, const __m128i& second)
{
if (Offset!=0)
first = _mm_alignr_epi8(second,first, Offset*4);
}
};
template<int Offset>
struct ei_palign_impl<Offset,__m128d>
{
EIGEN_STRONG_INLINE static void run(__m128d& first, const __m128d& second)
{
if (Offset==1)
first = _mm_castsi128_pd(_mm_alignr_epi8(_mm_castpd_si128(second), _mm_castpd_si128(first), 8));
}
};
#else
// SSE2 versions
template<int Offset>
struct ei_palign_impl<Offset,__m128>
{
EIGEN_STRONG_INLINE static void run(__m128& first, const __m128& second)
{
if (Offset==1)
{
first = _mm_move_ss(first,second);
first = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(first),0x39));
}
else if (Offset==2)
{
first = _mm_movehl_ps(first,first);
first = _mm_movelh_ps(first,second);
}
else if (Offset==3)
{
first = _mm_move_ss(first,second);
first = _mm_shuffle_ps(first,second,0x93);
}
}
};
template<int Offset>
struct ei_palign_impl<Offset,__m128i>
{
EIGEN_STRONG_INLINE static void run(__m128i& first, const __m128i& second)
{
if (Offset==1)
{
first = _mm_castps_si128(_mm_move_ss(_mm_castsi128_ps(first),_mm_castsi128_ps(second)));
first = _mm_shuffle_epi32(first,0x39);
}
else if (Offset==2)
{
first = _mm_castps_si128(_mm_movehl_ps(_mm_castsi128_ps(first),_mm_castsi128_ps(first)));
first = _mm_castps_si128(_mm_movelh_ps(_mm_castsi128_ps(first),_mm_castsi128_ps(second)));
}
else if (Offset==3)
{
first = _mm_castps_si128(_mm_move_ss(_mm_castsi128_ps(first),_mm_castsi128_ps(second)));
first = _mm_castps_si128(_mm_shuffle_ps(_mm_castsi128_ps(first),_mm_castsi128_ps(second),0x93));
}
}
};
template<int Offset>
struct ei_palign_impl<Offset,__m128d>
{
EIGEN_STRONG_INLINE static void run(__m128d& first, const __m128d& second)
{
if (Offset==1)
{
first = _mm_castps_pd(_mm_movehl_ps(_mm_castpd_ps(first),_mm_castpd_ps(first)));
first = _mm_castps_pd(_mm_movelh_ps(_mm_castpd_ps(first),_mm_castpd_ps(second)));
}
}
};
#endif
#define ei_vec4f_swizzle1(v,p,q,r,s) \
(_mm_castsi128_ps(_mm_shuffle_epi32( _mm_castps_si128(v), ((s)<<6|(r)<<4|(q)<<2|(p)))))
#endif // EIGEN_PACKET_MATH_SSE_H

173
ground/openpilotgcs/src/libs/eigen/Eigen/src/Geometry/AlignedBox.h Normal file
View File

@ -0,0 +1,173 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_ALIGNEDBOX_H
#define EIGEN_ALIGNEDBOX_H
/** \geometry_module \ingroup Geometry_Module
* \nonstableyet
*
* \class AlignedBox
*
* \brief An axis aligned box
*
* \param _Scalar the type of the scalar coefficients
* \param _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic.
*
* This class represents an axis aligned box as a pair of the minimal and maximal corners.
*/
template <typename _Scalar, int _AmbientDim>
class AlignedBox
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim==Dynamic ? Dynamic : _AmbientDim+1)
enum { AmbientDimAtCompileTime = _AmbientDim };
typedef _Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType;
/** Default constructor initializing a null box. */
inline explicit AlignedBox()
{ if (AmbientDimAtCompileTime!=Dynamic) setNull(); }
/** Constructs a null box with \a _dim the dimension of the ambient space. */
inline explicit AlignedBox(int _dim) : m_min(_dim), m_max(_dim)
{ setNull(); }
/** Constructs a box with extremities \a _min and \a _max. */
inline AlignedBox(const VectorType& _min, const VectorType& _max) : m_min(_min), m_max(_max) {}
/** Constructs a box containing a single point \a p. */
inline explicit AlignedBox(const VectorType& p) : m_min(p), m_max(p) {}
~AlignedBox() {}
/** \returns the dimension in which the box holds */
inline int dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size()-1 : AmbientDimAtCompileTime; }
/** \returns true if the box is null, i.e, empty. */
inline bool isNull() const { return (m_min.cwise() > m_max).any(); }
/** Makes \c *this a null/empty box. */
inline void setNull()
{
m_min.setConstant( std::numeric_limits<Scalar>::max());
m_max.setConstant(-std::numeric_limits<Scalar>::max());
}
/** \returns the minimal corner */
inline const VectorType& min() const { return m_min; }
/** \returns a non const reference to the minimal corner */
inline VectorType& min() { return m_min; }
/** \returns the maximal corner */
inline const VectorType& max() const { return m_max; }
/** \returns a non const reference to the maximal corner */
inline VectorType& max() { return m_max; }
/** \returns true if the point \a p is inside the box \c *this. */
inline bool contains(const VectorType& p) const
{ return (m_min.cwise()<=p).all() && (p.cwise()<=m_max).all(); }
/** \returns true if the box \a b is entirely inside the box \c *this. */
inline bool contains(const AlignedBox& b) const
{ return (m_min.cwise()<=b.min()).all() && (b.max().cwise()<=m_max).all(); }
/** Extends \c *this such that it contains the point \a p and returns a reference to \c *this. */
inline AlignedBox& extend(const VectorType& p)
{ m_min = m_min.cwise().min(p); m_max = m_max.cwise().max(p); return *this; }
/** Extends \c *this such that it contains the box \a b and returns a reference to \c *this. */
inline AlignedBox& extend(const AlignedBox& b)
{ m_min = m_min.cwise().min(b.m_min); m_max = m_max.cwise().max(b.m_max); return *this; }
/** Clamps \c *this by the box \a b and returns a reference to \c *this. */
inline AlignedBox& clamp(const AlignedBox& b)
{ m_min = m_min.cwise().max(b.m_min); m_max = m_max.cwise().min(b.m_max); return *this; }
/** Translate \c *this by the vector \a t and returns a reference to \c *this. */
inline AlignedBox& translate(const VectorType& t)
{ m_min += t; m_max += t; return *this; }
/** \returns the squared distance between the point \a p and the box \c *this,
* and zero if \a p is inside the box.
* \sa exteriorDistance()
*/
inline Scalar squaredExteriorDistance(const VectorType& p) const;
/** \returns the distance between the point \a p and the box \c *this,
* and zero if \a p is inside the box.
* \sa squaredExteriorDistance()
*/
inline Scalar exteriorDistance(const VectorType& p) const
{ return ei_sqrt(squaredExteriorDistance(p)); }
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template<typename NewScalarType>
inline typename ei_cast_return_type<AlignedBox,
AlignedBox<NewScalarType,AmbientDimAtCompileTime> >::type cast() const
{
return typename ei_cast_return_type<AlignedBox,
AlignedBox<NewScalarType,AmbientDimAtCompileTime> >::type(*this);
}
/** Copy constructor with scalar type conversion */
template<typename OtherScalarType>
inline explicit AlignedBox(const AlignedBox<OtherScalarType,AmbientDimAtCompileTime>& other)
{
m_min = other.min().template cast<Scalar>();
m_max = other.max().template cast<Scalar>();
}
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
bool isApprox(const AlignedBox& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
{ return m_min.isApprox(other.m_min, prec) && m_max.isApprox(other.m_max, prec); }
protected:
VectorType m_min, m_max;
};
template<typename Scalar,int AmbiantDim>
inline Scalar AlignedBox<Scalar,AmbiantDim>::squaredExteriorDistance(const VectorType& p) const
{
Scalar dist2 = 0.;
Scalar aux;
for (int k=0; k<dim(); ++k)
{
if ((aux = (p[k]-m_min[k]))<0.)
dist2 += aux*aux;
else if ( (aux = (m_max[k]-p[k]))<0. )
dist2 += aux*aux;
}
return dist2;
}
#endif // EIGEN_ALIGNEDBOX_H

228
ground/openpilotgcs/src/libs/eigen/Eigen/src/Geometry/AngleAxis.h Normal file
View File

@ -0,0 +1,228 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_ANGLEAXIS_H
#define EIGEN_ANGLEAXIS_H
/** \geometry_module \ingroup Geometry_Module
*
* \class AngleAxis
*
* \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis
*
* \param _Scalar the scalar type, i.e., the type of the coefficients.
*
* The following two typedefs are provided for convenience:
* \li \c AngleAxisf for \c float
* \li \c AngleAxisd for \c double
*
* \addexample AngleAxisForEuler \label How to define a rotation from Euler-angles
*
* Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily
* mimic Euler-angles. Here is an example:
* \include AngleAxis_mimic_euler.cpp
* Output: \verbinclude AngleAxis_mimic_euler.out
*
* \note This class is not aimed to be used to store a rotation transformation,
* but rather to make easier the creation of other rotation (Quaternion, rotation Matrix)
* and transformation objects.
*
* \sa class Quaternion, class Transform, MatrixBase::UnitX()
*/
template<typename _Scalar> struct ei_traits<AngleAxis<_Scalar> >
{
typedef _Scalar Scalar;
};
template<typename _Scalar>
class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3>
{
typedef RotationBase<AngleAxis<_Scalar>,3> Base;
public:
using Base::operator*;
enum { Dim = 3 };
/** the scalar type of the coefficients */
typedef _Scalar Scalar;
typedef Matrix<Scalar,3,3> Matrix3;
typedef Matrix<Scalar,3,1> Vector3;
typedef Quaternion<Scalar> QuaternionType;
protected:
Vector3 m_axis;
Scalar m_angle;
public:
/** Default constructor without initialization. */
AngleAxis() {}
/** Constructs and initialize the angle-axis rotation from an \a angle in radian
* and an \a axis which must be normalized. */
template<typename Derived>
inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
/** Constructs and initialize the angle-axis rotation from a quaternion \a q. */
inline AngleAxis(const QuaternionType& q) { *this = q; }
/** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */
template<typename Derived>
inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
Scalar angle() const { return m_angle; }
Scalar& angle() { return m_angle; }
const Vector3& axis() const { return m_axis; }
Vector3& axis() { return m_axis; }
/** Concatenates two rotations */
inline QuaternionType operator* (const AngleAxis& other) const
{ return QuaternionType(*this) * QuaternionType(other); }
/** Concatenates two rotations */
inline QuaternionType operator* (const QuaternionType& other) const
{ return QuaternionType(*this) * other; }
/** Concatenates two rotations */
friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
{ return a * QuaternionType(b); }
/** Concatenates two rotations */
inline Matrix3 operator* (const Matrix3& other) const
{ return toRotationMatrix() * other; }
/** Concatenates two rotations */
inline friend Matrix3 operator* (const Matrix3& a, const AngleAxis& b)
{ return a * b.toRotationMatrix(); }
/** Applies rotation to vector */
inline Vector3 operator* (const Vector3& other) const
{ return toRotationMatrix() * other; }
/** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */
AngleAxis inverse() const
{ return AngleAxis(-m_angle, m_axis); }
AngleAxis& operator=(const QuaternionType& q);
template<typename Derived>
AngleAxis& operator=(const MatrixBase<Derived>& m);
template<typename Derived>
AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
Matrix3 toRotationMatrix(void) const;
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template<typename NewScalarType>
inline typename ei_cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const
{ return typename ei_cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); }
/** Copy constructor with scalar type conversion */
template<typename OtherScalarType>
inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
{
m_axis = other.axis().template cast<Scalar>();
m_angle = Scalar(other.angle());
}
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
bool isApprox(const AngleAxis& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
{ return m_axis.isApprox(other.m_axis, prec) && ei_isApprox(m_angle,other.m_angle, prec); }
};
/** \ingroup Geometry_Module
* single precision angle-axis type */
typedef AngleAxis<float> AngleAxisf;
/** \ingroup Geometry_Module
* double precision angle-axis type */
typedef AngleAxis<double> AngleAxisd;
/** Set \c *this from a quaternion.
* The axis is normalized.
*/
template<typename Scalar>
AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionType& q)
{
Scalar n2 = q.vec().squaredNorm();
if (n2 < precision<Scalar>()*precision<Scalar>())
{
m_angle = 0;
m_axis << 1, 0, 0;
}
else
{
m_angle = 2*std::acos(q.w());
m_axis = q.vec() / ei_sqrt(n2);
}
return *this;
}
/** Set \c *this from a 3x3 rotation matrix \a mat.
*/
template<typename Scalar>
template<typename Derived>
AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat)
{
// Since a direct conversion would not be really faster,
// let's use the robust Quaternion implementation:
return *this = QuaternionType(mat);
}
/** Constructs and \returns an equivalent 3x3 rotation matrix.
*/
template<typename Scalar>
typename AngleAxis<Scalar>::Matrix3
AngleAxis<Scalar>::toRotationMatrix(void) const
{
Matrix3 res;
Vector3 sin_axis = ei_sin(m_angle) * m_axis;
Scalar c = ei_cos(m_angle);
Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
Scalar tmp;
tmp = cos1_axis.x() * m_axis.y();
res.coeffRef(0,1) = tmp - sin_axis.z();
res.coeffRef(1,0) = tmp + sin_axis.z();
tmp = cos1_axis.x() * m_axis.z();
res.coeffRef(0,2) = tmp + sin_axis.y();
res.coeffRef(2,0) = tmp - sin_axis.y();
tmp = cos1_axis.y() * m_axis.z();
res.coeffRef(1,2) = tmp - sin_axis.x();
res.coeffRef(2,1) = tmp + sin_axis.x();
res.diagonal() = (cos1_axis.cwise() * m_axis).cwise() + c;
return res;
}
#endif // EIGEN_ANGLEAXIS_H

6
ground/openpilotgcs/src/libs/eigen/Eigen/src/Geometry/CMakeLists.txt Normal file
View File

@ -0,0 +1,6 @@
FILE(GLOB Eigen_Geometry_SRCS "*.h")
INSTALL(FILES
${Eigen_Geometry_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Geometry
)

96
ground/openpilotgcs/src/libs/eigen/Eigen/src/Geometry/EulerAngles.h Normal file
View File

@ -0,0 +1,96 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_EULERANGLES_H
#define EIGEN_EULERANGLES_H
/** \geometry_module \ingroup Geometry_Module
* \nonstableyet
*
* \returns the Euler-angles of the rotation matrix \c *this using the convention defined by the triplet (\a a0,\a a1,\a a2)
*
* Each of the three parameters \a a0,\a a1,\a a2 represents the respective rotation axis as an integer in {0,1,2}.
* For instance, in:
* \code Vector3f ea = mat.eulerAngles(2, 0, 2); \endcode
* "2" represents the z axis and "0" the x axis, etc. The returned angles are such that
* we have the following equality:
* \code
* mat == AngleAxisf(ea[0], Vector3f::UnitZ())
* * AngleAxisf(ea[1], Vector3f::UnitX())
* * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode
* This corresponds to the right-multiply conventions (with right hand side frames).
*/
template<typename Derived>
inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
MatrixBase<Derived>::eulerAngles(int a0, int a1, int a2) const
{
/* Implemented from Graphics Gems IV */
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3)
Matrix<Scalar,3,1> res;
typedef Matrix<typename Derived::Scalar,2,1> Vector2;
const Scalar epsilon = precision<Scalar>();
const int odd = ((a0+1)%3 == a1) ? 0 : 1;
const int i = a0;
const int j = (a0 + 1 + odd)%3;
const int k = (a0 + 2 - odd)%3;
if (a0==a2)
{
Scalar s = Vector2(coeff(j,i) , coeff(k,i)).norm();
res[1] = ei_atan2(s, coeff(i,i));
if (s > epsilon)
{
res[0] = ei_atan2(coeff(j,i), coeff(k,i));
res[2] = ei_atan2(coeff(i,j),-coeff(i,k));
}
else
{
res[0] = Scalar(0);
res[2] = (coeff(i,i)>0?1:-1)*ei_atan2(-coeff(k,j), coeff(j,j));
}
}
else
{
Scalar c = Vector2(coeff(i,i) , coeff(i,j)).norm();
res[1] = ei_atan2(-coeff(i,k), c);
if (c > epsilon)
{
res[0] = ei_atan2(coeff(j,k), coeff(k,k));
res[2] = ei_atan2(coeff(i,j), coeff(i,i));
}
else
{
res[0] = Scalar(0);
res[2] = (coeff(i,k)>0?1:-1)*ei_atan2(-coeff(k,j), coeff(j,j));
}
}
if (!odd)
res = -res;
return res;
}
#endif // EIGEN_EULERANGLES_H

268
ground/openpilotgcs/src/libs/eigen/Eigen/src/Geometry/Hyperplane.h Normal file
View File

@ -0,0 +1,268 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_HYPERPLANE_H
#define EIGEN_HYPERPLANE_H
/** \geometry_module \ingroup Geometry_Module
*
* \class Hyperplane
*
* \brief A hyperplane
*
* A hyperplane is an affine subspace of dimension n-1 in a space of dimension n.
* For example, a hyperplane in a plane is a line; a hyperplane in 3-space is a plane.
*
* \param _Scalar the scalar type, i.e., the type of the coefficients
* \param _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic.
* Notice that the dimension of the hyperplane is _AmbientDim-1.
*
* This class represents an hyperplane as the zero set of the implicit equation
* \f$ n \cdot x + d = 0 \f$ where \f$ n \f$ is a unit normal vector of the plane (linear part)
* and \f$ d \f$ is the distance (offset) to the origin.
*/
template <typename _Scalar, int _AmbientDim>
class Hyperplane
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim==Dynamic ? Dynamic : _AmbientDim+1)
enum { AmbientDimAtCompileTime = _AmbientDim };
typedef _Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType;
typedef Matrix<Scalar,int(AmbientDimAtCompileTime)==Dynamic
? Dynamic
: int(AmbientDimAtCompileTime)+1,1> Coefficients;
typedef Block<Coefficients,AmbientDimAtCompileTime,1> NormalReturnType;
/** Default constructor without initialization */
inline explicit Hyperplane() {}
/** Constructs a dynamic-size hyperplane with \a _dim the dimension
* of the ambient space */
inline explicit Hyperplane(int _dim) : m_coeffs(_dim+1) {}
/** Construct a plane from its normal \a n and a point \a e onto the plane.
* \warning the vector normal is assumed to be normalized.
*/
inline Hyperplane(const VectorType& n, const VectorType& e)
: m_coeffs(n.size()+1)
{
normal() = n;
offset() = -e.dot(n);
}
/** Constructs a plane from its normal \a n and distance to the origin \a d
* such that the algebraic equation of the plane is \f$ n \cdot x + d = 0 \f$.
* \warning the vector normal is assumed to be normalized.
*/
inline Hyperplane(const VectorType& n, Scalar d)
: m_coeffs(n.size()+1)
{
normal() = n;
offset() = d;
}
/** Constructs a hyperplane passing through the two points. If the dimension of the ambient space
* is greater than 2, then there isn't uniqueness, so an arbitrary choice is made.
*/
static inline Hyperplane Through(const VectorType& p0, const VectorType& p1)
{
Hyperplane result(p0.size());
result.normal() = (p1 - p0).unitOrthogonal();
result.offset() = -result.normal().dot(p0);
return result;
}
/** Constructs a hyperplane passing through the three points. The dimension of the ambient space
* is required to be exactly 3.
*/
static inline Hyperplane Through(const VectorType& p0, const VectorType& p1, const VectorType& p2)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 3)
Hyperplane result(p0.size());
result.normal() = (p2 - p0).cross(p1 - p0).normalized();
result.offset() = -result.normal().dot(p0);
return result;
}
/** Constructs a hyperplane passing through the parametrized line \a parametrized.
* If the dimension of the ambient space is greater than 2, then there isn't uniqueness,
* so an arbitrary choice is made.
*/
// FIXME to be consitent with the rest this could be implemented as a static Through function ??
explicit Hyperplane(const ParametrizedLine<Scalar, AmbientDimAtCompileTime>& parametrized)
{
normal() = parametrized.direction().unitOrthogonal();
offset() = -normal().dot(parametrized.origin());
}
~Hyperplane() {}
/** \returns the dimension in which the plane holds */
inline int dim() const { return AmbientDimAtCompileTime==Dynamic ? m_coeffs.size()-1 : AmbientDimAtCompileTime; }
/** normalizes \c *this */
void normalize(void)
{
m_coeffs /= normal().norm();
}
/** \returns the signed distance between the plane \c *this and a point \a p.
* \sa absDistance()
*/
inline Scalar signedDistance(const VectorType& p) const { return p.dot(normal()) + offset(); }
/** \returns the absolute distance between the plane \c *this and a point \a p.
* \sa signedDistance()
*/
inline Scalar absDistance(const VectorType& p) const { return ei_abs(signedDistance(p)); }
/** \returns the projection of a point \a p onto the plane \c *this.
*/
inline VectorType projection(const VectorType& p) const { return p - signedDistance(p) * normal(); }
/** \returns a constant reference to the unit normal vector of the plane, which corresponds
* to the linear part of the implicit equation.
*/
inline const NormalReturnType normal() const { return NormalReturnType(m_coeffs,0,0,dim(),1); }
/** \returns a non-constant reference to the unit normal vector of the plane, which corresponds
* to the linear part of the implicit equation.
*/
inline NormalReturnType normal() { return NormalReturnType(m_coeffs,0,0,dim(),1); }
/** \returns the distance to the origin, which is also the "constant term" of the implicit equation
* \warning the vector normal is assumed to be normalized.
*/
inline const Scalar& offset() const { return m_coeffs.coeff(dim()); }
/** \returns a non-constant reference to the distance to the origin, which is also the constant part
* of the implicit equation */
inline Scalar& offset() { return m_coeffs(dim()); }
/** \returns a constant reference to the coefficients c_i of the plane equation:
* \f$ c_0*x_0 + ... + c_{d-1}*x_{d-1} + c_d = 0 \f$
*/
inline const Coefficients& coeffs() const { return m_coeffs; }
/** \returns a non-constant reference to the coefficients c_i of the plane equation:
* \f$ c_0*x_0 + ... + c_{d-1}*x_{d-1} + c_d = 0 \f$
*/
inline Coefficients& coeffs() { return m_coeffs; }
/** \returns the intersection of *this with \a other.
*
* \warning The ambient space must be a plane, i.e. have dimension 2, so that \c *this and \a other are lines.
*
* \note If \a other is approximately parallel to *this, this method will return any point on *this.
*/
VectorType intersection(const Hyperplane& other)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2)
Scalar det = coeffs().coeff(0) * other.coeffs().coeff(1) - coeffs().coeff(1) * other.coeffs().coeff(0);
// since the line equations ax+by=c are normalized with a^2+b^2=1, the following tests
// whether the two lines are approximately parallel.
if(ei_isMuchSmallerThan(det, Scalar(1)))
{ // special case where the two lines are approximately parallel. Pick any point on the first line.
if(ei_abs(coeffs().coeff(1))>ei_abs(coeffs().coeff(0)))
return VectorType(coeffs().coeff(1), -coeffs().coeff(2)/coeffs().coeff(1)-coeffs().coeff(0));
else
return VectorType(-coeffs().coeff(2)/coeffs().coeff(0)-coeffs().coeff(1), coeffs().coeff(0));
}
else
{ // general case
Scalar invdet = Scalar(1) / det;
return VectorType(invdet*(coeffs().coeff(1)*other.coeffs().coeff(2)-other.coeffs().coeff(1)*coeffs().coeff(2)),
invdet*(other.coeffs().coeff(0)*coeffs().coeff(2)-coeffs().coeff(0)*other.coeffs().coeff(2)));
}
}
/** Applies the transformation matrix \a mat to \c *this and returns a reference to \c *this.
*
* \param mat the Dim x Dim transformation matrix
* \param traits specifies whether the matrix \a mat represents an Isometry
* or a more generic Affine transformation. The default is Affine.
*/
template<typename XprType>
inline Hyperplane& transform(const MatrixBase<XprType>& mat, TransformTraits traits = Affine)
{
if (traits==Affine)
normal() = mat.inverse().transpose() * normal();
else if (traits==Isometry)
normal() = mat * normal();
else
{
ei_assert("invalid traits value in Hyperplane::transform()");
}
return *this;
}
/** Applies the transformation \a t to \c *this and returns a reference to \c *this.
*
* \param t the transformation of dimension Dim
* \param traits specifies whether the transformation \a t represents an Isometry
* or a more generic Affine transformation. The default is Affine.
* Other kind of transformations are not supported.
*/
inline Hyperplane& transform(const Transform<Scalar,AmbientDimAtCompileTime>& t,
TransformTraits traits = Affine)
{
transform(t.linear(), traits);
offset() -= t.translation().dot(normal());
return *this;
}
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template<typename NewScalarType>
inline typename ei_cast_return_type<Hyperplane,
Hyperplane<NewScalarType,AmbientDimAtCompileTime> >::type cast() const
{
return typename ei_cast_return_type<Hyperplane,
Hyperplane<NewScalarType,AmbientDimAtCompileTime> >::type(*this);
}
/** Copy constructor with scalar type conversion */
template<typename OtherScalarType>
inline explicit Hyperplane(const Hyperplane<OtherScalarType,AmbientDimAtCompileTime>& other)
{ m_coeffs = other.coeffs().template cast<Scalar>(); }
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
bool isApprox(const Hyperplane& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
{ return m_coeffs.isApprox(other.m_coeffs, prec); }
protected:
Coefficients m_coeffs;
};
#endif // EIGEN_HYPERPLANE_H

119
ground/openpilotgcs/src/libs/eigen/Eigen/src/Geometry/OrthoMethods.h Normal file
View File

@ -0,0 +1,119 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_ORTHOMETHODS_H
#define EIGEN_ORTHOMETHODS_H
/** \geometry_module
*
* \returns the cross product of \c *this and \a other
*
* Here is a very good explanation of cross-product: http://xkcd.com/199/
*/
template<typename Derived>
template<typename OtherDerived>
inline typename MatrixBase<Derived>::PlainMatrixType
MatrixBase<Derived>::cross(const MatrixBase<OtherDerived>& other) const
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,3)
// Note that there is no need for an expression here since the compiler
// optimize such a small temporary very well (even within a complex expression)
const typename ei_nested<Derived,2>::type lhs(derived());
const typename ei_nested<OtherDerived,2>::type rhs(other.derived());
return typename ei_plain_matrix_type<Derived>::type(
lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1),
lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2),
lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0)
);
}
template<typename Derived, int Size = Derived::SizeAtCompileTime>
struct ei_unitOrthogonal_selector
{
typedef typename ei_plain_matrix_type<Derived>::type VectorType;
typedef typename ei_traits<Derived>::Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
inline static VectorType run(const Derived& src)
{
VectorType perp(src.size());
/* Let us compute the crossed product of *this with a vector
* that is not too close to being colinear to *this.
*/
/* unless the x and y coords are both close to zero, we can
* simply take ( -y, x, 0 ) and normalize it.
*/
if((!ei_isMuchSmallerThan(src.x(), src.z()))
|| (!ei_isMuchSmallerThan(src.y(), src.z())))
{
RealScalar invnm = RealScalar(1)/src.template start<2>().norm();
perp.coeffRef(0) = -ei_conj(src.y())*invnm;
perp.coeffRef(1) = ei_conj(src.x())*invnm;
perp.coeffRef(2) = 0;
}
/* if both x and y are close to zero, then the vector is close
* to the z-axis, so it's far from colinear to the x-axis for instance.
* So we take the crossed product with (1,0,0) and normalize it.
*/
else
{
RealScalar invnm = RealScalar(1)/src.template end<2>().norm();
perp.coeffRef(0) = 0;
perp.coeffRef(1) = -ei_conj(src.z())*invnm;
perp.coeffRef(2) = ei_conj(src.y())*invnm;
}
if( (Derived::SizeAtCompileTime!=Dynamic && Derived::SizeAtCompileTime>3)
|| (Derived::SizeAtCompileTime==Dynamic && src.size()>3) )
perp.end(src.size()-3).setZero();
return perp;
}
};
template<typename Derived>
struct ei_unitOrthogonal_selector<Derived,2>
{
typedef typename ei_plain_matrix_type<Derived>::type VectorType;
inline static VectorType run(const Derived& src)
{ return VectorType(-ei_conj(src.y()), ei_conj(src.x())).normalized(); }
};
/** \returns a unit vector which is orthogonal to \c *this
*
* The size of \c *this must be at least 2. If the size is exactly 2,
* then the returned vector is a counter clock wise rotation of \c *this, i.e., (-y,x).normalized().
*
* \sa cross()
*/
template<typename Derived>
typename MatrixBase<Derived>::PlainMatrixType
MatrixBase<Derived>::unitOrthogonal() const
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return ei_unitOrthogonal_selector<Derived>::run(derived());
}
#endif // EIGEN_ORTHOMETHODS_H

155
ground/openpilotgcs/src/libs/eigen/Eigen/src/Geometry/ParametrizedLine.h Normal file
View File

@ -0,0 +1,155 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_PARAMETRIZEDLINE_H
#define EIGEN_PARAMETRIZEDLINE_H
/** \geometry_module \ingroup Geometry_Module
*
* \class ParametrizedLine
*
* \brief A parametrized line
*
* A parametrized line is defined by an origin point \f$ \mathbf{o} \f$ and a unit
* direction vector \f$ \mathbf{d} \f$ such that the line corresponds to
* the set \f$ l(t) = \mathbf{o} + t \mathbf{d} \f$, \f$ l \in \mathbf{R} \f$.
*
* \param _Scalar the scalar type, i.e., the type of the coefficients
* \param _AmbientDim the dimension of the ambient space, can be a compile time value or Dynamic.
*/
template <typename _Scalar, int _AmbientDim>
class ParametrizedLine
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim)
enum { AmbientDimAtCompileTime = _AmbientDim };
typedef _Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
typedef Matrix<Scalar,AmbientDimAtCompileTime,1> VectorType;
/** Default constructor without initialization */
inline explicit ParametrizedLine() {}
/** Constructs a dynamic-size line with \a _dim the dimension
* of the ambient space */
inline explicit ParametrizedLine(int _dim) : m_origin(_dim), m_direction(_dim) {}
/** Initializes a parametrized line of direction \a direction and origin \a origin.
* \warning the vector direction is assumed to be normalized.
*/
ParametrizedLine(const VectorType& origin, const VectorType& direction)
: m_origin(origin), m_direction(direction) {}
explicit ParametrizedLine(const Hyperplane<_Scalar, _AmbientDim>& hyperplane);
/** Constructs a parametrized line going from \a p0 to \a p1. */
static inline ParametrizedLine Through(const VectorType& p0, const VectorType& p1)
{ return ParametrizedLine(p0, (p1-p0).normalized()); }
~ParametrizedLine() {}
/** \returns the dimension in which the line holds */
inline int dim() const { return m_direction.size(); }
const VectorType& origin() const { return m_origin; }
VectorType& origin() { return m_origin; }
const VectorType& direction() const { return m_direction; }
VectorType& direction() { return m_direction; }
/** \returns the squared distance of a point \a p to its projection onto the line \c *this.
* \sa distance()
*/
RealScalar squaredDistance(const VectorType& p) const
{
VectorType diff = p-origin();
return (diff - diff.dot(direction())* direction()).squaredNorm();
}
/** \returns the distance of a point \a p to its projection onto the line \c *this.
* \sa squaredDistance()
*/
RealScalar distance(const VectorType& p) const { return ei_sqrt(squaredDistance(p)); }
/** \returns the projection of a point \a p onto the line \c *this. */
VectorType projection(const VectorType& p) const
{ return origin() + (p-origin()).dot(direction()) * direction(); }
Scalar intersection(const Hyperplane<_Scalar, _AmbientDim>& hyperplane);
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template<typename NewScalarType>
inline typename ei_cast_return_type<ParametrizedLine,
ParametrizedLine<NewScalarType,AmbientDimAtCompileTime> >::type cast() const
{
return typename ei_cast_return_type<ParametrizedLine,
ParametrizedLine<NewScalarType,AmbientDimAtCompileTime> >::type(*this);
}
/** Copy constructor with scalar type conversion */
template<typename OtherScalarType>
inline explicit ParametrizedLine(const ParametrizedLine<OtherScalarType,AmbientDimAtCompileTime>& other)
{
m_origin = other.origin().template cast<Scalar>();
m_direction = other.direction().template cast<Scalar>();
}
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
bool isApprox(const ParametrizedLine& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
{ return m_origin.isApprox(other.m_origin, prec) && m_direction.isApprox(other.m_direction, prec); }
protected:
VectorType m_origin, m_direction;
};
/** Constructs a parametrized line from a 2D hyperplane
*
* \warning the ambient space must have dimension 2 such that the hyperplane actually describes a line
*/
template <typename _Scalar, int _AmbientDim>
inline ParametrizedLine<_Scalar, _AmbientDim>::ParametrizedLine(const Hyperplane<_Scalar, _AmbientDim>& hyperplane)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 2)
direction() = hyperplane.normal().unitOrthogonal();
origin() = -hyperplane.normal()*hyperplane.offset();
}
/** \returns the parameter value of the intersection between \c *this and the given hyperplane
*/
template <typename _Scalar, int _AmbientDim>
inline _Scalar ParametrizedLine<_Scalar, _AmbientDim>::intersection(const Hyperplane<_Scalar, _AmbientDim>& hyperplane)
{
return -(hyperplane.offset()+origin().dot(hyperplane.normal()))
/(direction().dot(hyperplane.normal()));
}
#endif // EIGEN_PARAMETRIZEDLINE_H

530
ground/openpilotgcs/src/libs/eigen/Eigen/src/Geometry/Quaternion.h Normal file
View File

@ -0,0 +1,530 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_QUATERNION_H
#define EIGEN_QUATERNION_H
template<typename Other,
int OtherRows=Other::RowsAtCompileTime,
int OtherCols=Other::ColsAtCompileTime>
struct ei_quaternion_assign_impl;
/** \geometry_module \ingroup Geometry_Module
*
* \class Quaternion
*
* \brief The quaternion class used to represent 3D orientations and rotations
*
* \param _Scalar the scalar type, i.e., the type of the coefficients
*
* This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of
* orientations and rotations of objects in three dimensions. Compared to other representations
* like Euler angles or 3x3 matrices, quatertions offer the following advantages:
* \li \b compact storage (4 scalars)
* \li \b efficient to compose (28 flops),
* \li \b stable spherical interpolation
*
* The following two typedefs are provided for convenience:
* \li \c Quaternionf for \c float
* \li \c Quaterniond for \c double
*
* \sa class AngleAxis, class Transform
*/
template<typename _Scalar> struct ei_traits<Quaternion<_Scalar> >
{
typedef _Scalar Scalar;
};
template<typename _Scalar>
class Quaternion : public RotationBase<Quaternion<_Scalar>,3>
{
typedef RotationBase<Quaternion<_Scalar>,3> Base;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,4)
using Base::operator*;
/** the scalar type of the coefficients */
typedef _Scalar Scalar;
/** the type of the Coefficients 4-vector */
typedef Matrix<Scalar, 4, 1> Coefficients;
/** the type of a 3D vector */
typedef Matrix<Scalar,3,1> Vector3;
/** the equivalent rotation matrix type */
typedef Matrix<Scalar,3,3> Matrix3;
/** the equivalent angle-axis type */
typedef AngleAxis<Scalar> AngleAxisType;
/** \returns the \c x coefficient */
inline Scalar x() const { return m_coeffs.coeff(0); }
/** \returns the \c y coefficient */
inline Scalar y() const { return m_coeffs.coeff(1); }
/** \returns the \c z coefficient */
inline Scalar z() const { return m_coeffs.coeff(2); }
/** \returns the \c w coefficient */
inline Scalar w() const { return m_coeffs.coeff(3); }
/** \returns a reference to the \c x coefficient */
inline Scalar& x() { return m_coeffs.coeffRef(0); }
/** \returns a reference to the \c y coefficient */
inline Scalar& y() { return m_coeffs.coeffRef(1); }
/** \returns a reference to the \c z coefficient */
inline Scalar& z() { return m_coeffs.coeffRef(2); }
/** \returns a reference to the \c w coefficient */
inline Scalar& w() { return m_coeffs.coeffRef(3); }
/** \returns a read-only vector expression of the imaginary part (x,y,z) */
inline const Block<Coefficients,3,1> vec() const { return m_coeffs.template start<3>(); }
/** \returns a vector expression of the imaginary part (x,y,z) */
inline Block<Coefficients,3,1> vec() { return m_coeffs.template start<3>(); }
/** \returns a read-only vector expression of the coefficients (x,y,z,w) */
inline const Coefficients& coeffs() const { return m_coeffs; }
/** \returns a vector expression of the coefficients (x,y,z,w) */
inline Coefficients& coeffs() { return m_coeffs; }
/** Default constructor leaving the quaternion uninitialized. */
inline Quaternion() {}
/** Constructs and initializes the quaternion \f$ w+xi+yj+zk \f$ from
* its four coefficients \a w, \a x, \a y and \a z.
*
* \warning Note the order of the arguments: the real \a w coefficient first,
* while internally the coefficients are stored in the following order:
* [\c x, \c y, \c z, \c w]
*/
inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z)
{ m_coeffs << x, y, z, w; }
/** Copy constructor */
inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; }
/** Constructs and initializes a quaternion from the angle-axis \a aa */
explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
/** Constructs and initializes a quaternion from either:
* - a rotation matrix expression,
* - a 4D vector expression representing quaternion coefficients.
* \sa operator=(MatrixBase<Derived>)
*/
template<typename Derived>
explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
Quaternion& operator=(const Quaternion& other);
Quaternion& operator=(const AngleAxisType& aa);
template<typename Derived>
Quaternion& operator=(const MatrixBase<Derived>& m);
/** \returns a quaternion representing an identity rotation
* \sa MatrixBase::Identity()
*/
inline static Quaternion Identity() { return Quaternion(1, 0, 0, 0); }
/** \sa Quaternion::Identity(), MatrixBase::setIdentity()
*/
inline Quaternion& setIdentity() { m_coeffs << 0, 0, 0, 1; return *this; }
/** \returns the squared norm of the quaternion's coefficients
* \sa Quaternion::norm(), MatrixBase::squaredNorm()
*/
inline Scalar squaredNorm() const { return m_coeffs.squaredNorm(); }
/** \returns the norm of the quaternion's coefficients
* \sa Quaternion::squaredNorm(), MatrixBase::norm()
*/
inline Scalar norm() const { return m_coeffs.norm(); }
/** Normalizes the quaternion \c *this
* \sa normalized(), MatrixBase::normalize() */
inline void normalize() { m_coeffs.normalize(); }
/** \returns a normalized version of \c *this
* \sa normalize(), MatrixBase::normalized() */
inline Quaternion normalized() const { return Quaternion(m_coeffs.normalized()); }
/** \returns the dot product of \c *this and \a other
* Geometrically speaking, the dot product of two unit quaternions
* corresponds to the cosine of half the angle between the two rotations.
* \sa angularDistance()
*/
inline Scalar dot(const Quaternion& other) const { return m_coeffs.dot(other.m_coeffs); }
inline Scalar angularDistance(const Quaternion& other) const;
Matrix3 toRotationMatrix(void) const;
template<typename Derived1, typename Derived2>
Quaternion& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
inline Quaternion operator* (const Quaternion& q) const;
inline Quaternion& operator*= (const Quaternion& q);
Quaternion inverse(void) const;
Quaternion conjugate(void) const;
Quaternion slerp(Scalar t, const Quaternion& other) const;
template<typename Derived>
Vector3 operator* (const MatrixBase<Derived>& vec) const;
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template<typename NewScalarType>
inline typename ei_cast_return_type<Quaternion,Quaternion<NewScalarType> >::type cast() const
{ return typename ei_cast_return_type<Quaternion,Quaternion<NewScalarType> >::type(*this); }
/** Copy constructor with scalar type conversion */
template<typename OtherScalarType>
inline explicit Quaternion(const Quaternion<OtherScalarType>& other)
{ m_coeffs = other.coeffs().template cast<Scalar>(); }
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
bool isApprox(const Quaternion& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
{ return m_coeffs.isApprox(other.m_coeffs, prec); }
protected:
Coefficients m_coeffs;
};
/** \ingroup Geometry_Module
* single precision quaternion type */
typedef Quaternion<float> Quaternionf;
/** \ingroup Geometry_Module
* double precision quaternion type */
typedef Quaternion<double> Quaterniond;
// Generic Quaternion * Quaternion product
template<int Arch,typename Scalar> inline Quaternion<Scalar>
ei_quaternion_product(const Quaternion<Scalar>& a, const Quaternion<Scalar>& b)
{
return Quaternion<Scalar>
(
a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
);
}
#ifdef EIGEN_VECTORIZE_SSE
template<> inline Quaternion<float>
ei_quaternion_product<EiArch_SSE,float>(const Quaternion<float>& _a, const Quaternion<float>& _b)
{
const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0,0,0,0x80000000));
Quaternion<float> res;
__m128 a = _a.coeffs().packet<Aligned>(0);
__m128 b = _b.coeffs().packet<Aligned>(0);
__m128 flip1 = _mm_xor_ps(_mm_mul_ps(ei_vec4f_swizzle1(a,1,2,0,2),
ei_vec4f_swizzle1(b,2,0,1,2)),mask);
__m128 flip2 = _mm_xor_ps(_mm_mul_ps(ei_vec4f_swizzle1(a,3,3,3,1),
ei_vec4f_swizzle1(b,0,1,2,1)),mask);
ei_pstore(&res.x(),
_mm_add_ps(_mm_sub_ps(_mm_mul_ps(a,ei_vec4f_swizzle1(b,3,3,3,3)),
_mm_mul_ps(ei_vec4f_swizzle1(a,2,0,1,0),
ei_vec4f_swizzle1(b,1,2,0,0))),
_mm_add_ps(flip1,flip2)));
return res;
}
#endif
/** \returns the concatenation of two rotations as a quaternion-quaternion product */
template <typename Scalar>
inline Quaternion<Scalar> Quaternion<Scalar>::operator* (const Quaternion& other) const
{
return ei_quaternion_product<EiArch>(*this,other);
}
/** \sa operator*(Quaternion) */
template <typename Scalar>
inline Quaternion<Scalar>& Quaternion<Scalar>::operator*= (const Quaternion& other)
{
return (*this = *this * other);
}
/** Rotation of a vector by a quaternion.
* \remarks If the quaternion is used to rotate several points (>1)
* then it is much more efficient to first convert it to a 3x3 Matrix.
* Comparison of the operation cost for n transformations:
* - Quaternion: 30n
* - Via a Matrix3: 24 + 15n
*/
template <typename Scalar>
template<typename Derived>
inline typename Quaternion<Scalar>::Vector3
Quaternion<Scalar>::operator* (const MatrixBase<Derived>& v) const
{
// Note that this algorithm comes from the optimization by hand
// of the conversion to a Matrix followed by a Matrix/Vector product.
// It appears to be much faster than the common algorithm found
// in the litterature (30 versus 39 flops). It also requires two
// Vector3 as temporaries.
Vector3 uv;
uv = 2 * this->vec().cross(v);
return v + this->w() * uv + this->vec().cross(uv);
}
template<typename Scalar>
inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const Quaternion& other)
{
m_coeffs = other.m_coeffs;
return *this;
}
/** Set \c *this from an angle-axis \a aa and returns a reference to \c *this
*/
template<typename Scalar>
inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const AngleAxisType& aa)
{
Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
this->w() = ei_cos(ha);
this->vec() = ei_sin(ha) * aa.axis();
return *this;
}
/** Set \c *this from the expression \a xpr:
* - if \a xpr is a 4x1 vector, then \a xpr is assumed to be a quaternion
* - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix
* and \a xpr is converted to a quaternion
*/
template<typename Scalar>
template<typename Derived>
inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const MatrixBase<Derived>& xpr)
{
ei_quaternion_assign_impl<Derived>::run(*this, xpr.derived());
return *this;
}
/** Convert the quaternion to a 3x3 rotation matrix */
template<typename Scalar>
inline typename Quaternion<Scalar>::Matrix3
Quaternion<Scalar>::toRotationMatrix(void) const
{
// NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
// if not inlined then the cost of the return by value is huge ~ +35%,
// however, not inlining this function is an order of magnitude slower, so
// it has to be inlined, and so the return by value is not an issue
Matrix3 res;
const Scalar tx = 2*this->x();
const Scalar ty = 2*this->y();
const Scalar tz = 2*this->z();
const Scalar twx = tx*this->w();
const Scalar twy = ty*this->w();
const Scalar twz = tz*this->w();
const Scalar txx = tx*this->x();
const Scalar txy = ty*this->x();
const Scalar txz = tz*this->x();
const Scalar tyy = ty*this->y();
const Scalar tyz = tz*this->y();
const Scalar tzz = tz*this->z();
res.coeffRef(0,0) = 1-(tyy+tzz);
res.coeffRef(0,1) = txy-twz;
res.coeffRef(0,2) = txz+twy;
res.coeffRef(1,0) = txy+twz;
res.coeffRef(1,1) = 1-(txx+tzz);
res.coeffRef(1,2) = tyz-twx;
res.coeffRef(2,0) = txz-twy;
res.coeffRef(2,1) = tyz+twx;
res.coeffRef(2,2) = 1-(txx+tyy);
return res;
}
/** Sets *this to be a quaternion representing a rotation sending the vector \a a to the vector \a b.
*
* \returns a reference to *this.
*
* Note that the two input vectors do \b not have to be normalized.
*/
template<typename Scalar>
template<typename Derived1, typename Derived2>
inline Quaternion<Scalar>& Quaternion<Scalar>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
{
Vector3 v0 = a.normalized();
Vector3 v1 = b.normalized();
Scalar c = v0.dot(v1);
// if dot == 1, vectors are the same
if (ei_isApprox(c,Scalar(1)))
{
// set to identity
this->w() = 1; this->vec().setZero();
return *this;
}
// if dot == -1, vectors are opposites
if (ei_isApprox(c,Scalar(-1)))
{
this->vec() = v0.unitOrthogonal();
this->w() = 0;
return *this;
}
Vector3 axis = v0.cross(v1);
Scalar s = ei_sqrt((Scalar(1)+c)*Scalar(2));
Scalar invs = Scalar(1)/s;
this->vec() = axis * invs;
this->w() = s * Scalar(0.5);
return *this;
}
/** \returns the multiplicative inverse of \c *this
* Note that in most cases, i.e., if you simply want the opposite rotation,
* and/or the quaternion is normalized, then it is enough to use the conjugate.
*
* \sa Quaternion::conjugate()
*/
template <typename Scalar>
inline Quaternion<Scalar> Quaternion<Scalar>::inverse() const
{
// FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite() ??
Scalar n2 = this->squaredNorm();
if (n2 > 0)
return Quaternion(conjugate().coeffs() / n2);
else
{
// return an invalid result to flag the error
return Quaternion(Coefficients::Zero());
}
}
/** \returns the conjugate of the \c *this which is equal to the multiplicative inverse
* if the quaternion is normalized.
* The conjugate of a quaternion represents the opposite rotation.
*
* \sa Quaternion::inverse()
*/
template <typename Scalar>
inline Quaternion<Scalar> Quaternion<Scalar>::conjugate() const
{
return Quaternion(this->w(),-this->x(),-this->y(),-this->z());
}
/** \returns the angle (in radian) between two rotations
* \sa dot()
*/
template <typename Scalar>
inline Scalar Quaternion<Scalar>::angularDistance(const Quaternion& other) const
{
double d = ei_abs(this->dot(other));
if (d>=1.0)
return 0;
return Scalar(2) * std::acos(d);
}
/** \returns the spherical linear interpolation between the two quaternions
* \c *this and \a other at the parameter \a t
*/
template <typename Scalar>
Quaternion<Scalar> Quaternion<Scalar>::slerp(Scalar t, const Quaternion& other) const
{
static const Scalar one = Scalar(1) - machine_epsilon<Scalar>();
Scalar d = this->dot(other);
Scalar absD = ei_abs(d);
Scalar scale0;
Scalar scale1;
if (absD>=one)
{
scale0 = Scalar(1) - t;
scale1 = t;
}
else
{
// theta is the angle between the 2 quaternions
Scalar theta = std::acos(absD);
Scalar sinTheta = ei_sin(theta);
scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta;
scale1 = ei_sin( ( t * theta) ) / sinTheta;
if (d<0)
scale1 = -scale1;
}
return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
}
// set from a rotation matrix
template<typename Other>
struct ei_quaternion_assign_impl<Other,3,3>
{
typedef typename Other::Scalar Scalar;
inline static void run(Quaternion<Scalar>& q, const Other& mat)
{
// This algorithm comes from "Quaternion Calculus and Fast Animation",
// Ken Shoemake, 1987 SIGGRAPH course notes
Scalar t = mat.trace();
if (t > 0)
{
t = ei_sqrt(t + Scalar(1.0));
q.w() = Scalar(0.5)*t;
t = Scalar(0.5)/t;
q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
}
else
{
int i = 0;
if (mat.coeff(1,1) > mat.coeff(0,0))
i = 1;
if (mat.coeff(2,2) > mat.coeff(i,i))
i = 2;
int j = (i+1)%3;
int k = (j+1)%3;
t = ei_sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
q.coeffs().coeffRef(i) = Scalar(0.5) * t;
t = Scalar(0.5)/t;
q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
}
}
};
// set from a vector of coefficients assumed to be a quaternion
template<typename Other>
struct ei_quaternion_assign_impl<Other,4,1>
{
typedef typename Other::Scalar Scalar;
inline static void run(Quaternion<Scalar>& q, const Other& vec)
{
q.coeffs() = vec;
}
};
#endif // EIGEN_QUATERNION_H

159
ground/openpilotgcs/src/libs/eigen/Eigen/src/Geometry/Rotation2D.h Normal file
View File

@ -0,0 +1,159 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_ROTATION2D_H
#define EIGEN_ROTATION2D_H
/** \geometry_module \ingroup Geometry_Module
*
* \class Rotation2D
*
* \brief Represents a rotation/orientation in a 2 dimensional space.
*
* \param _Scalar the scalar type, i.e., the type of the coefficients
*
* This class is equivalent to a single scalar representing a counter clock wise rotation
* as a single angle in radian. It provides some additional features such as the automatic
* conversion from/to a 2x2 rotation matrix. Moreover this class aims to provide a similar
* interface to Quaternion in order to facilitate the writing of generic algorithms
* dealing with rotations.
*
* \sa class Quaternion, class Transform
*/
template<typename _Scalar> struct ei_traits<Rotation2D<_Scalar> >
{
typedef _Scalar Scalar;
};
template<typename _Scalar>
class Rotation2D : public RotationBase<Rotation2D<_Scalar>,2>
{
typedef RotationBase<Rotation2D<_Scalar>,2> Base;
public:
using Base::operator*;
enum { Dim = 2 };
/** the scalar type of the coefficients */
typedef _Scalar Scalar;
typedef Matrix<Scalar,2,1> Vector2;
typedef Matrix<Scalar,2,2> Matrix2;
protected:
Scalar m_angle;
public:
/** Construct a 2D counter clock wise rotation from the angle \a a in radian. */
inline Rotation2D(Scalar a) : m_angle(a) {}
/** \returns the rotation angle */
inline Scalar angle() const { return m_angle; }
/** \returns a read-write reference to the rotation angle */
inline Scalar& angle() { return m_angle; }
/** \returns the inverse rotation */
inline Rotation2D inverse() const { return -m_angle; }
/** Concatenates two rotations */
inline Rotation2D operator*(const Rotation2D& other) const
{ return m_angle + other.m_angle; }
/** Concatenates two rotations */
inline Rotation2D& operator*=(const Rotation2D& other)
{ return m_angle += other.m_angle; return *this; }
/** Applies the rotation to a 2D vector */
Vector2 operator* (const Vector2& vec) const
{ return toRotationMatrix() * vec; }
template<typename Derived>
Rotation2D& fromRotationMatrix(const MatrixBase<Derived>& m);
Matrix2 toRotationMatrix(void) const;
/** \returns the spherical interpolation between \c *this and \a other using
* parameter \a t. It is in fact equivalent to a linear interpolation.
*/
inline Rotation2D slerp(Scalar t, const Rotation2D& other) const
{ return m_angle * (1-t) + other.angle() * t; }
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template<typename NewScalarType>
inline typename ei_cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type cast() const
{ return typename ei_cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type(*this); }
/** Copy constructor with scalar type conversion */
template<typename OtherScalarType>
inline explicit Rotation2D(const Rotation2D<OtherScalarType>& other)
{
m_angle = Scalar(other.angle());
}
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
bool isApprox(const Rotation2D& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
{ return ei_isApprox(m_angle,other.m_angle, prec); }
};
/** \ingroup Geometry_Module
* single precision 2D rotation type */
typedef Rotation2D<float> Rotation2Df;
/** \ingroup Geometry_Module
* double precision 2D rotation type */
typedef Rotation2D<double> Rotation2Dd;
/** Set \c *this from a 2x2 rotation matrix \a mat.
* In other words, this function extract the rotation angle
* from the rotation matrix.
*/
template<typename Scalar>
template<typename Derived>
Rotation2D<Scalar>& Rotation2D<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
{
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==2 && Derived::ColsAtCompileTime==2,YOU_MADE_A_PROGRAMMING_MISTAKE)
m_angle = ei_atan2(mat.coeff(1,0), mat.coeff(0,0));
return *this;
}
/** Constructs and \returns an equivalent 2x2 rotation matrix.
*/
template<typename Scalar>
typename Rotation2D<Scalar>::Matrix2
Rotation2D<Scalar>::toRotationMatrix(void) const
{
Scalar sinA = ei_sin(m_angle);
Scalar cosA = ei_cos(m_angle);
return (Matrix2() << cosA, -sinA, sinA, cosA).finished();
}
#endif // EIGEN_ROTATION2D_H

137
ground/openpilotgcs/src/libs/eigen/Eigen/src/Geometry/RotationBase.h Normal file
View File

@ -0,0 +1,137 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_ROTATIONBASE_H
#define EIGEN_ROTATIONBASE_H
// this file aims to contains the various representations of rotation/orientation
// in 2D and 3D space excepted Matrix and Quaternion.
/** \class RotationBase
*
* \brief Common base class for compact rotation representations
*
* \param Derived is the derived type, i.e., a rotation type
* \param _Dim the dimension of the space
*/
template<typename Derived, int _Dim>
class RotationBase
{
public:
enum { Dim = _Dim };
/** the scalar type of the coefficients */
typedef typename ei_traits<Derived>::Scalar Scalar;
/** corresponding linear transformation matrix type */
typedef Matrix<Scalar,Dim,Dim> RotationMatrixType;
inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
inline Derived& derived() { return *static_cast<Derived*>(this); }
/** \returns an equivalent rotation matrix */
inline RotationMatrixType toRotationMatrix() const { return derived().toRotationMatrix(); }
/** \returns the inverse rotation */
inline Derived inverse() const { return derived().inverse(); }
/** \returns the concatenation of the rotation \c *this with a translation \a t */
inline Transform<Scalar,Dim> operator*(const Translation<Scalar,Dim>& t) const
{ return toRotationMatrix() * t; }
/** \returns the concatenation of the rotation \c *this with a scaling \a s */
inline RotationMatrixType operator*(const Scaling<Scalar,Dim>& s) const
{ return toRotationMatrix() * s; }
/** \returns the concatenation of the rotation \c *this with an affine transformation \a t */
inline Transform<Scalar,Dim> operator*(const Transform<Scalar,Dim>& t) const
{ return toRotationMatrix() * t; }
};
/** \geometry_module
*
* Constructs a Dim x Dim rotation matrix from the rotation \a r
*/
template<typename _Scalar, int _Rows, int _Cols, int _Storage, int _MaxRows, int _MaxCols>
template<typename OtherDerived>
Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols>
::Matrix(const RotationBase<OtherDerived,ColsAtCompileTime>& r)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,int(OtherDerived::Dim),int(OtherDerived::Dim))
*this = r.toRotationMatrix();
}
/** \geometry_module
*
* Set a Dim x Dim rotation matrix from the rotation \a r
*/
template<typename _Scalar, int _Rows, int _Cols, int _Storage, int _MaxRows, int _MaxCols>
template<typename OtherDerived>
Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols>&
Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols>
::operator=(const RotationBase<OtherDerived,ColsAtCompileTime>& r)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,int(OtherDerived::Dim),int(OtherDerived::Dim))
return *this = r.toRotationMatrix();
}
/** \internal
*
* Helper function to return an arbitrary rotation object to a rotation matrix.
*
* \param Scalar the numeric type of the matrix coefficients
* \param Dim the dimension of the current space
*
* It returns a Dim x Dim fixed size matrix.
*
* Default specializations are provided for:
* - any scalar type (2D),
* - any matrix expression,
* - any type based on RotationBase (e.g., Quaternion, AngleAxis, Rotation2D)
*
* Currently ei_toRotationMatrix is only used by Transform.
*
* \sa class Transform, class Rotation2D, class Quaternion, class AngleAxis
*/
template<typename Scalar, int Dim>
inline static Matrix<Scalar,2,2> ei_toRotationMatrix(const Scalar& s)
{
EIGEN_STATIC_ASSERT(Dim==2,YOU_MADE_A_PROGRAMMING_MISTAKE)
return Rotation2D<Scalar>(s).toRotationMatrix();
}
template<typename Scalar, int Dim, typename OtherDerived>
inline static Matrix<Scalar,Dim,Dim> ei_toRotationMatrix(const RotationBase<OtherDerived,Dim>& r)
{
return r.toRotationMatrix();
}
template<typename Scalar, int Dim, typename OtherDerived>
inline static const MatrixBase<OtherDerived>& ei_toRotationMatrix(const MatrixBase<OtherDerived>& mat)
{
EIGEN_STATIC_ASSERT(OtherDerived::RowsAtCompileTime==Dim && OtherDerived::ColsAtCompileTime==Dim,
YOU_MADE_A_PROGRAMMING_MISTAKE)
return mat;
}
#endif // EIGEN_ROTATIONBASE_H

181
ground/openpilotgcs/src/libs/eigen/Eigen/src/Geometry/Scaling.h Normal file
View File

@ -0,0 +1,181 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_SCALING_H
#define EIGEN_SCALING_H
/** \geometry_module \ingroup Geometry_Module
*
* \class Scaling
*
* \brief Represents a possibly non uniform scaling transformation
*
* \param _Scalar the scalar type, i.e., the type of the coefficients.
* \param _Dim the dimension of the space, can be a compile time value or Dynamic
*
* \note This class is not aimed to be used to store a scaling transformation,
* but rather to make easier the constructions and updates of Transform objects.
*
* \sa class Translation, class Transform
*/
template<typename _Scalar, int _Dim>
class Scaling
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Dim)
/** dimension of the space */
enum { Dim = _Dim };
/** the scalar type of the coefficients */
typedef _Scalar Scalar;
/** corresponding vector type */
typedef Matrix<Scalar,Dim,1> VectorType;
/** corresponding linear transformation matrix type */
typedef Matrix<Scalar,Dim,Dim> LinearMatrixType;
/** corresponding translation type */
typedef Translation<Scalar,Dim> TranslationType;
/** corresponding affine transformation type */
typedef Transform<Scalar,Dim> TransformType;
protected:
VectorType m_coeffs;
public:
/** Default constructor without initialization. */
Scaling() {}
/** Constructs and initialize a uniform scaling transformation */
explicit inline Scaling(const Scalar& s) { m_coeffs.setConstant(s); }
/** 2D only */
inline Scaling(const Scalar& sx, const Scalar& sy)
{
ei_assert(Dim==2);
m_coeffs.x() = sx;
m_coeffs.y() = sy;
}
/** 3D only */
inline Scaling(const Scalar& sx, const Scalar& sy, const Scalar& sz)
{
ei_assert(Dim==3);
m_coeffs.x() = sx;
m_coeffs.y() = sy;
m_coeffs.z() = sz;
}
/** Constructs and initialize the scaling transformation from a vector of scaling coefficients */
explicit inline Scaling(const VectorType& coeffs) : m_coeffs(coeffs) {}
const VectorType& coeffs() const { return m_coeffs; }
VectorType& coeffs() { return m_coeffs; }
/** Concatenates two scaling */
inline Scaling operator* (const Scaling& other) const
{ return Scaling(coeffs().cwise() * other.coeffs()); }
/** Concatenates a scaling and a translation */
inline TransformType operator* (const TranslationType& t) const;
/** Concatenates a scaling and an affine transformation */
inline TransformType operator* (const TransformType& t) const;
/** Concatenates a scaling and a linear transformation matrix */
// TODO returns an expression
inline LinearMatrixType operator* (const LinearMatrixType& other) const
{ return coeffs().asDiagonal() * other; }
/** Concatenates a linear transformation matrix and a scaling */
// TODO returns an expression
friend inline LinearMatrixType operator* (const LinearMatrixType& other, const Scaling& s)
{ return other * s.coeffs().asDiagonal(); }
template<typename Derived>
inline LinearMatrixType operator*(const RotationBase<Derived,Dim>& r) const
{ return *this * r.toRotationMatrix(); }
/** Applies scaling to vector */
inline VectorType operator* (const VectorType& other) const
{ return coeffs().asDiagonal() * other; }
/** \returns the inverse scaling */
inline Scaling inverse() const
{ return Scaling(coeffs().cwise().inverse()); }
inline Scaling& operator=(const Scaling& other)
{
m_coeffs = other.m_coeffs;
return *this;
}
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template<typename NewScalarType>
inline typename ei_cast_return_type<Scaling,Scaling<NewScalarType,Dim> >::type cast() const
{ return typename ei_cast_return_type<Scaling,Scaling<NewScalarType,Dim> >::type(*this); }
/** Copy constructor with scalar type conversion */
template<typename OtherScalarType>
inline explicit Scaling(const Scaling<OtherScalarType,Dim>& other)
{ m_coeffs = other.coeffs().template cast<Scalar>(); }
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
bool isApprox(const Scaling& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
{ return m_coeffs.isApprox(other.m_coeffs, prec); }
};
/** \addtogroup Geometry_Module */
//@{
typedef Scaling<float, 2> Scaling2f;
typedef Scaling<double,2> Scaling2d;
typedef Scaling<float, 3> Scaling3f;
typedef Scaling<double,3> Scaling3d;
//@}
template<typename Scalar, int Dim>
inline typename Scaling<Scalar,Dim>::TransformType
Scaling<Scalar,Dim>::operator* (const TranslationType& t) const
{
TransformType res;
res.matrix().setZero();
res.linear().diagonal() = coeffs();
res.translation() = m_coeffs.cwise() * t.vector();
res(Dim,Dim) = Scalar(1);
return res;
}
template<typename Scalar, int Dim>
inline typename Scaling<Scalar,Dim>::TransformType
Scaling<Scalar,Dim>::operator* (const TransformType& t) const
{
TransformType res = t;
res.prescale(m_coeffs);
return res;
}
#endif // EIGEN_SCALING_H

785
ground/openpilotgcs/src/libs/eigen/Eigen/src/Geometry/Transform.h Normal file
View File

@ -0,0 +1,785 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_TRANSFORM_H
#define EIGEN_TRANSFORM_H
/** Represents some traits of a transformation */
enum TransformTraits {
Isometry, ///< the transformation is a concatenation of translations and rotations
Affine, ///< the transformation is affine (linear transformation + translation)
Projective ///< the transformation might not be affine
};
// Note that we have to pass Dim and HDim because it is not allowed to use a template
// parameter to define a template specialization. To be more precise, in the following
// specializations, it is not allowed to use Dim+1 instead of HDim.
template< typename Other,
int Dim,
int HDim,
int OtherRows=Other::RowsAtCompileTime,
int OtherCols=Other::ColsAtCompileTime>
struct ei_transform_product_impl;
/** \geometry_module \ingroup Geometry_Module
*
* \class Transform
*
* \brief Represents an homogeneous transformation in a N dimensional space
*
* \param _Scalar the scalar type, i.e., the type of the coefficients
* \param _Dim the dimension of the space
*
* The homography is internally represented and stored as a (Dim+1)^2 matrix which
* is available through the matrix() method.
*
* Conversion methods from/to Qt's QMatrix and QTransform are available if the
* preprocessor token EIGEN_QT_SUPPORT is defined.
*
* \sa class Matrix, class Quaternion
*/
template<typename _Scalar, int _Dim>
class Transform
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Dim==Dynamic ? Dynamic : (_Dim+1)*(_Dim+1))
enum {
Dim = _Dim, ///< space dimension in which the transformation holds
HDim = _Dim+1 ///< size of a respective homogeneous vector
};
/** the scalar type of the coefficients */
typedef _Scalar Scalar;
/** type of the matrix used to represent the transformation */
typedef Matrix<Scalar,HDim,HDim> MatrixType;
/** type of the matrix used to represent the linear part of the transformation */
typedef Matrix<Scalar,Dim,Dim> LinearMatrixType;
/** type of read/write reference to the linear part of the transformation */
typedef Block<MatrixType,Dim,Dim> LinearPart;
/** type of a vector */
typedef Matrix<Scalar,Dim,1> VectorType;
/** type of a read/write reference to the translation part of the rotation */
typedef Block<MatrixType,Dim,1> TranslationPart;
/** corresponding translation type */
typedef Translation<Scalar,Dim> TranslationType;
/** corresponding scaling transformation type */
typedef Scaling<Scalar,Dim> ScalingType;
protected:
MatrixType m_matrix;
public:
/** Default constructor without initialization of the coefficients. */
inline Transform() { }
inline Transform(const Transform& other)
{
m_matrix = other.m_matrix;
}
inline explicit Transform(const TranslationType& t) { *this = t; }
inline explicit Transform(const ScalingType& s) { *this = s; }
template<typename Derived>
inline explicit Transform(const RotationBase<Derived, Dim>& r) { *this = r; }
inline Transform& operator=(const Transform& other)
{ m_matrix = other.m_matrix; return *this; }
template<typename OtherDerived, bool BigMatrix> // MSVC 2005 will commit suicide if BigMatrix has a default value
struct construct_from_matrix
{
static inline void run(Transform *transform, const MatrixBase<OtherDerived>& other)
{
transform->matrix() = other;
}
};
template<typename OtherDerived> struct construct_from_matrix<OtherDerived, true>
{
static inline void run(Transform *transform, const MatrixBase<OtherDerived>& other)
{
transform->linear() = other;
transform->translation().setZero();
transform->matrix()(Dim,Dim) = Scalar(1);
transform->matrix().template block<1,Dim>(Dim,0).setZero();
}
};
/** Constructs and initializes a transformation from a Dim^2 or a (Dim+1)^2 matrix. */
template<typename OtherDerived>
inline explicit Transform(const MatrixBase<OtherDerived>& other)
{
construct_from_matrix<OtherDerived, int(OtherDerived::RowsAtCompileTime) == Dim>::run(this, other);
}
/** Set \c *this from a (Dim+1)^2 matrix. */
template<typename OtherDerived>
inline Transform& operator=(const MatrixBase<OtherDerived>& other)
{ m_matrix = other; return *this; }
#ifdef EIGEN_QT_SUPPORT
inline Transform(const QMatrix& other);
inline Transform& operator=(const QMatrix& other);
inline QMatrix toQMatrix(void) const;
inline Transform(const QTransform& other);
inline Transform& operator=(const QTransform& other);
inline QTransform toQTransform(void) const;
#endif
/** shortcut for m_matrix(row,col);
* \sa MatrixBase::operaror(int,int) const */
inline Scalar operator() (int row, int col) const { return m_matrix(row,col); }
/** shortcut for m_matrix(row,col);
* \sa MatrixBase::operaror(int,int) */
inline Scalar& operator() (int row, int col) { return m_matrix(row,col); }
/** \returns a read-only expression of the transformation matrix */
inline const MatrixType& matrix() const { return m_matrix; }
/** \returns a writable expression of the transformation matrix */
inline MatrixType& matrix() { return m_matrix; }
/** \returns a read-only expression of the linear (linear) part of the transformation */
inline const LinearPart linear() const { return m_matrix.template block<Dim,Dim>(0,0); }
/** \returns a writable expression of the linear (linear) part of the transformation */
inline LinearPart linear() { return m_matrix.template block<Dim,Dim>(0,0); }
/** \returns a read-only expression of the translation vector of the transformation */
inline const TranslationPart translation() const { return m_matrix.template block<Dim,1>(0,Dim); }
/** \returns a writable expression of the translation vector of the transformation */
inline TranslationPart translation() { return m_matrix.template block<Dim,1>(0,Dim); }
/** \returns an expression of the product between the transform \c *this and a matrix expression \a other
*
* The right hand side \a other might be either:
* \li a vector of size Dim,
* \li an homogeneous vector of size Dim+1,
* \li a transformation matrix of size Dim+1 x Dim+1.
*/
// note: this function is defined here because some compilers cannot find the respective declaration
template<typename OtherDerived>
inline const typename ei_transform_product_impl<OtherDerived,_Dim,_Dim+1>::ResultType
operator * (const MatrixBase<OtherDerived> &other) const
{ return ei_transform_product_impl<OtherDerived,Dim,HDim>::run(*this,other.derived()); }
/** \returns the product expression of a transformation matrix \a a times a transform \a b
* The transformation matrix \a a must have a Dim+1 x Dim+1 sizes. */
template<typename OtherDerived>
friend inline const typename ProductReturnType<OtherDerived,MatrixType>::Type
operator * (const MatrixBase<OtherDerived> &a, const Transform &b)
{ return a.derived() * b.matrix(); }
/** Contatenates two transformations */
inline const Transform
operator * (const Transform& other) const
{ return Transform(m_matrix * other.matrix()); }
/** \sa MatrixBase::setIdentity() */
void setIdentity() { m_matrix.setIdentity(); }
static const typename MatrixType::IdentityReturnType Identity()
{
return MatrixType::Identity();
}
template<typename OtherDerived>
inline Transform& scale(const MatrixBase<OtherDerived> &other);
template<typename OtherDerived>
inline Transform& prescale(const MatrixBase<OtherDerived> &other);
inline Transform& scale(Scalar s);
inline Transform& prescale(Scalar s);
template<typename OtherDerived>
inline Transform& translate(const MatrixBase<OtherDerived> &other);
template<typename OtherDerived>
inline Transform& pretranslate(const MatrixBase<OtherDerived> &other);
template<typename RotationType>
inline Transform& rotate(const RotationType& rotation);
template<typename RotationType>
inline Transform& prerotate(const RotationType& rotation);
Transform& shear(Scalar sx, Scalar sy);
Transform& preshear(Scalar sx, Scalar sy);
inline Transform& operator=(const TranslationType& t);
inline Transform& operator*=(const TranslationType& t) { return translate(t.vector()); }
inline Transform operator*(const TranslationType& t) const;
inline Transform& operator=(const ScalingType& t);
inline Transform& operator*=(const ScalingType& s) { return scale(s.coeffs()); }
inline Transform operator*(const ScalingType& s) const;
friend inline Transform operator*(const LinearMatrixType& mat, const Transform& t)
{
Transform res = t;
res.matrix().row(Dim) = t.matrix().row(Dim);
res.matrix().template block<Dim,HDim>(0,0) = (mat * t.matrix().template block<Dim,HDim>(0,0)).lazy();
return res;
}
template<typename Derived>
inline Transform& operator=(const RotationBase<Derived,Dim>& r);
template<typename Derived>
inline Transform& operator*=(const RotationBase<Derived,Dim>& r) { return rotate(r.toRotationMatrix()); }
template<typename Derived>
inline Transform operator*(const RotationBase<Derived,Dim>& r) const;
LinearMatrixType rotation() const;
template<typename RotationMatrixType, typename ScalingMatrixType>
void computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const;
template<typename ScalingMatrixType, typename RotationMatrixType>
void computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const;
template<typename PositionDerived, typename OrientationType, typename ScaleDerived>
Transform& fromPositionOrientationScale(const MatrixBase<PositionDerived> &position,
const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale);
inline const MatrixType inverse(TransformTraits traits = Affine) const;
/** \returns a const pointer to the column major internal matrix */
const Scalar* data() const { return m_matrix.data(); }
/** \returns a non-const pointer to the column major internal matrix */
Scalar* data() { return m_matrix.data(); }
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template<typename NewScalarType>
inline typename ei_cast_return_type<Transform,Transform<NewScalarType,Dim> >::type cast() const
{ return typename ei_cast_return_type<Transform,Transform<NewScalarType,Dim> >::type(*this); }
/** Copy constructor with scalar type conversion */
template<typename OtherScalarType>
inline explicit Transform(const Transform<OtherScalarType,Dim>& other)
{ m_matrix = other.matrix().template cast<Scalar>(); }
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
bool isApprox(const Transform& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
{ return m_matrix.isApprox(other.m_matrix, prec); }
#ifdef EIGEN_TRANSFORM_PLUGIN
#include EIGEN_TRANSFORM_PLUGIN
#endif
protected:
};
/** \ingroup Geometry_Module */
typedef Transform<float,2> Transform2f;
/** \ingroup Geometry_Module */
typedef Transform<float,3> Transform3f;
/** \ingroup Geometry_Module */
typedef Transform<double,2> Transform2d;
/** \ingroup Geometry_Module */
typedef Transform<double,3> Transform3d;
/**************************
*** Optional QT support ***
**************************/
#ifdef EIGEN_QT_SUPPORT
/** Initialises \c *this from a QMatrix assuming the dimension is 2.
*
* This function is available only if the token EIGEN_QT_SUPPORT is defined.
*/
template<typename Scalar, int Dim>
Transform<Scalar,Dim>::Transform(const QMatrix& other)
{
*this = other;
}
/** Set \c *this from a QMatrix assuming the dimension is 2.
*
* This function is available only if the token EIGEN_QT_SUPPORT is defined.
*/
template<typename Scalar, int Dim>
Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const QMatrix& other)
{
EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
m_matrix << other.m11(), other.m21(), other.dx(),
other.m12(), other.m22(), other.dy(),
0, 0, 1;
return *this;
}
/** \returns a QMatrix from \c *this assuming the dimension is 2.
*
* \warning this convertion might loss data if \c *this is not affine
*
* This function is available only if the token EIGEN_QT_SUPPORT is defined.
*/
template<typename Scalar, int Dim>
QMatrix Transform<Scalar,Dim>::toQMatrix(void) const
{
EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
return QMatrix(m_matrix.coeff(0,0), m_matrix.coeff(1,0),
m_matrix.coeff(0,1), m_matrix.coeff(1,1),
m_matrix.coeff(0,2), m_matrix.coeff(1,2));
}
/** Initialises \c *this from a QTransform assuming the dimension is 2.
*
* This function is available only if the token EIGEN_QT_SUPPORT is defined.
*/
template<typename Scalar, int Dim>
Transform<Scalar,Dim>::Transform(const QTransform& other)
{
*this = other;
}
/** Set \c *this from a QTransform assuming the dimension is 2.
*
* This function is available only if the token EIGEN_QT_SUPPORT is defined.
*/
template<typename Scalar, int Dim>
Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const QTransform& other)
{
EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
m_matrix << other.m11(), other.m21(), other.dx(),
other.m12(), other.m22(), other.dy(),
other.m13(), other.m23(), other.m33();
return *this;
}
/** \returns a QTransform from \c *this assuming the dimension is 2.
*
* This function is available only if the token EIGEN_QT_SUPPORT is defined.
*/
template<typename Scalar, int Dim>
QTransform Transform<Scalar,Dim>::toQTransform(void) const
{
EIGEN_STATIC_ASSERT(Dim==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
return QTransform(m_matrix.coeff(0,0), m_matrix.coeff(1,0), m_matrix.coeff(2,0),
m_matrix.coeff(0,1), m_matrix.coeff(1,1), m_matrix.coeff(2,1),
m_matrix.coeff(0,2), m_matrix.coeff(1,2), m_matrix.coeff(2,2));
}
#endif
/*********************
*** Procedural API ***
*********************/
/** Applies on the right the non uniform scale transformation represented
* by the vector \a other to \c *this and returns a reference to \c *this.
* \sa prescale()
*/
template<typename Scalar, int Dim>
template<typename OtherDerived>
Transform<Scalar,Dim>&
Transform<Scalar,Dim>::scale(const MatrixBase<OtherDerived> &other)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
linear() = (linear() * other.asDiagonal()).lazy();
return *this;
}
/** Applies on the right a uniform scale of a factor \a c to \c *this
* and returns a reference to \c *this.
* \sa prescale(Scalar)
*/
template<typename Scalar, int Dim>
inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::scale(Scalar s)
{
linear() *= s;
return *this;
}
/** Applies on the left the non uniform scale transformation represented
* by the vector \a other to \c *this and returns a reference to \c *this.
* \sa scale()
*/
template<typename Scalar, int Dim>
template<typename OtherDerived>
Transform<Scalar,Dim>&
Transform<Scalar,Dim>::prescale(const MatrixBase<OtherDerived> &other)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
m_matrix.template block<Dim,HDim>(0,0) = (other.asDiagonal() * m_matrix.template block<Dim,HDim>(0,0)).lazy();
return *this;
}
/** Applies on the left a uniform scale of a factor \a c to \c *this
* and returns a reference to \c *this.
* \sa scale(Scalar)
*/
template<typename Scalar, int Dim>
inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::prescale(Scalar s)
{
m_matrix.template corner<Dim,HDim>(TopLeft) *= s;
return *this;
}
/** Applies on the right the translation matrix represented by the vector \a other
* to \c *this and returns a reference to \c *this.
* \sa pretranslate()
*/
template<typename Scalar, int Dim>
template<typename OtherDerived>
Transform<Scalar,Dim>&
Transform<Scalar,Dim>::translate(const MatrixBase<OtherDerived> &other)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
translation() += linear() * other;
return *this;
}
/** Applies on the left the translation matrix represented by the vector \a other
* to \c *this and returns a reference to \c *this.
* \sa translate()
*/
template<typename Scalar, int Dim>
template<typename OtherDerived>
Transform<Scalar,Dim>&
Transform<Scalar,Dim>::pretranslate(const MatrixBase<OtherDerived> &other)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived,int(Dim))
translation() += other;
return *this;
}
/** Applies on the right the rotation represented by the rotation \a rotation
* to \c *this and returns a reference to \c *this.
*
* The template parameter \a RotationType is the type of the rotation which
* must be known by ei_toRotationMatrix<>.
*
* Natively supported types includes:
* - any scalar (2D),
* - a Dim x Dim matrix expression,
* - a Quaternion (3D),
* - a AngleAxis (3D)
*
* This mechanism is easily extendable to support user types such as Euler angles,
* or a pair of Quaternion for 4D rotations.
*
* \sa rotate(Scalar), class Quaternion, class AngleAxis, prerotate(RotationType)
*/
template<typename Scalar, int Dim>
template<typename RotationType>
Transform<Scalar,Dim>&
Transform<Scalar,Dim>::rotate(const RotationType& rotation)
{
linear() *= ei_toRotationMatrix<Scalar,Dim>(rotation);
return *this;
}
/** Applies on the left the rotation represented by the rotation \a rotation
* to \c *this and returns a reference to \c *this.
*
* See rotate() for further details.
*
* \sa rotate()
*/
template<typename Scalar, int Dim>
template<typename RotationType>
Transform<Scalar,Dim>&
Transform<Scalar,Dim>::prerotate(const RotationType& rotation)
{
m_matrix.template block<Dim,HDim>(0,0) = ei_toRotationMatrix<Scalar,Dim>(rotation)
* m_matrix.template block<Dim,HDim>(0,0);
return *this;
}
/** Applies on the right the shear transformation represented
* by the vector \a other to \c *this and returns a reference to \c *this.
* \warning 2D only.
* \sa preshear()
*/
template<typename Scalar, int Dim>
Transform<Scalar,Dim>&
Transform<Scalar,Dim>::shear(Scalar sx, Scalar sy)
{
EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
VectorType tmp = linear().col(0)*sy + linear().col(1);
linear() << linear().col(0) + linear().col(1)*sx, tmp;
return *this;
}
/** Applies on the left the shear transformation represented
* by the vector \a other to \c *this and returns a reference to \c *this.
* \warning 2D only.
* \sa shear()
*/
template<typename Scalar, int Dim>
Transform<Scalar,Dim>&
Transform<Scalar,Dim>::preshear(Scalar sx, Scalar sy)
{
EIGEN_STATIC_ASSERT(int(Dim)==2, YOU_MADE_A_PROGRAMMING_MISTAKE)
m_matrix.template block<Dim,HDim>(0,0) = LinearMatrixType(1, sx, sy, 1) * m_matrix.template block<Dim,HDim>(0,0);
return *this;
}
/******************************************************
*** Scaling, Translation and Rotation compatibility ***
******************************************************/
template<typename Scalar, int Dim>
inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const TranslationType& t)
{
linear().setIdentity();
translation() = t.vector();
m_matrix.template block<1,Dim>(Dim,0).setZero();
m_matrix(Dim,Dim) = Scalar(1);
return *this;
}
template<typename Scalar, int Dim>
inline Transform<Scalar,Dim> Transform<Scalar,Dim>::operator*(const TranslationType& t) const
{
Transform res = *this;
res.translate(t.vector());
return res;
}
template<typename Scalar, int Dim>
inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const ScalingType& s)
{
m_matrix.setZero();
linear().diagonal() = s.coeffs();
m_matrix.coeffRef(Dim,Dim) = Scalar(1);
return *this;
}
template<typename Scalar, int Dim>
inline Transform<Scalar,Dim> Transform<Scalar,Dim>::operator*(const ScalingType& s) const
{
Transform res = *this;
res.scale(s.coeffs());
return res;
}
template<typename Scalar, int Dim>
template<typename Derived>
inline Transform<Scalar,Dim>& Transform<Scalar,Dim>::operator=(const RotationBase<Derived,Dim>& r)
{
linear() = ei_toRotationMatrix<Scalar,Dim>(r);
translation().setZero();
m_matrix.template block<1,Dim>(Dim,0).setZero();
m_matrix.coeffRef(Dim,Dim) = Scalar(1);
return *this;
}
template<typename Scalar, int Dim>
template<typename Derived>
inline Transform<Scalar,Dim> Transform<Scalar,Dim>::operator*(const RotationBase<Derived,Dim>& r) const
{
Transform res = *this;
res.rotate(r.derived());
return res;
}
/************************
*** Special functions ***
************************/
/** \returns the rotation part of the transformation
* \nonstableyet
*
* \svd_module
*
* \sa computeRotationScaling(), computeScalingRotation(), class SVD
*/
template<typename Scalar, int Dim>
typename Transform<Scalar,Dim>::LinearMatrixType
Transform<Scalar,Dim>::rotation() const
{
LinearMatrixType result;
computeRotationScaling(&result, (LinearMatrixType*)0);
return result;
}
/** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being
* not necessarily positive.
*
* If either pointer is zero, the corresponding computation is skipped.
*
* \nonstableyet
*
* \svd_module
*
* \sa computeScalingRotation(), rotation(), class SVD
*/
template<typename Scalar, int Dim>
template<typename RotationMatrixType, typename ScalingMatrixType>
void Transform<Scalar,Dim>::computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const
{
linear().svd().computeRotationScaling(rotation, scaling);
}
/** decomposes the linear part of the transformation as a product rotation x scaling, the scaling being
* not necessarily positive.
*
* If either pointer is zero, the corresponding computation is skipped.
*
* \nonstableyet
*
* \svd_module
*
* \sa computeRotationScaling(), rotation(), class SVD
*/
template<typename Scalar, int Dim>
template<typename ScalingMatrixType, typename RotationMatrixType>
void Transform<Scalar,Dim>::computeScalingRotation(ScalingMatrixType *scaling, RotationMatrixType *rotation) const
{
linear().svd().computeScalingRotation(scaling, rotation);
}
/** Convenient method to set \c *this from a position, orientation and scale
* of a 3D object.
*/
template<typename Scalar, int Dim>
template<typename PositionDerived, typename OrientationType, typename ScaleDerived>
Transform<Scalar,Dim>&
Transform<Scalar,Dim>::fromPositionOrientationScale(const MatrixBase<PositionDerived> &position,
const OrientationType& orientation, const MatrixBase<ScaleDerived> &scale)
{
linear() = ei_toRotationMatrix<Scalar,Dim>(orientation);
linear() *= scale.asDiagonal();
translation() = position;
m_matrix.template block<1,Dim>(Dim,0).setZero();
m_matrix(Dim,Dim) = Scalar(1);
return *this;
}
/** \nonstableyet
*
* \returns the inverse transformation matrix according to some given knowledge
* on \c *this.
*
* \param traits allows to optimize the inversion process when the transformion
* is known to be not a general transformation. The possible values are:
* - Projective if the transformation is not necessarily affine, i.e., if the
* last row is not guaranteed to be [0 ... 0 1]
* - Affine is the default, the last row is assumed to be [0 ... 0 1]
* - Isometry if the transformation is only a concatenations of translations
* and rotations.
*
* \warning unless \a traits is always set to NoShear or NoScaling, this function
* requires the generic inverse method of MatrixBase defined in the LU module. If
* you forget to include this module, then you will get hard to debug linking errors.
*
* \sa MatrixBase::inverse()
*/
template<typename Scalar, int Dim>
inline const typename Transform<Scalar,Dim>::MatrixType
Transform<Scalar,Dim>::inverse(TransformTraits traits) const
{
if (traits == Projective)
{
return m_matrix.inverse();
}
else
{
MatrixType res;
if (traits == Affine)
{
res.template corner<Dim,Dim>(TopLeft) = linear().inverse();
}
else if (traits == Isometry)
{
res.template corner<Dim,Dim>(TopLeft) = linear().transpose();
}
else
{
ei_assert("invalid traits value in Transform::inverse()");
}
// translation and remaining parts
res.template corner<Dim,1>(TopRight) = - res.template corner<Dim,Dim>(TopLeft) * translation();
res.template corner<1,Dim>(BottomLeft).setZero();
res.coeffRef(Dim,Dim) = Scalar(1);
return res;
}
}
/*****************************************************
*** Specializations of operator* with a MatrixBase ***
*****************************************************/
template<typename Other, int Dim, int HDim>
struct ei_transform_product_impl<Other,Dim,HDim, HDim,HDim>
{
typedef Transform<typename Other::Scalar,Dim> TransformType;
typedef typename TransformType::MatrixType MatrixType;
typedef typename ProductReturnType<MatrixType,Other>::Type ResultType;
static ResultType run(const TransformType& tr, const Other& other)
{ return tr.matrix() * other; }
};
template<typename Other, int Dim, int HDim>
struct ei_transform_product_impl<Other,Dim,HDim, Dim,Dim>
{
typedef Transform<typename Other::Scalar,Dim> TransformType;
typedef typename TransformType::MatrixType MatrixType;
typedef TransformType ResultType;
static ResultType run(const TransformType& tr, const Other& other)
{
TransformType res;
res.translation() = tr.translation();
res.matrix().row(Dim) = tr.matrix().row(Dim);
res.linear() = (tr.linear() * other).lazy();
return res;
}
};
template<typename Other, int Dim, int HDim>
struct ei_transform_product_impl<Other,Dim,HDim, HDim,1>
{
typedef Transform<typename Other::Scalar,Dim> TransformType;
typedef typename TransformType::MatrixType MatrixType;
typedef typename ProductReturnType<MatrixType,Other>::Type ResultType;
static ResultType run(const TransformType& tr, const Other& other)
{ return tr.matrix() * other; }
};
template<typename Other, int Dim, int HDim>
struct ei_transform_product_impl<Other,Dim,HDim, Dim,1>
{
typedef typename Other::Scalar Scalar;
typedef Transform<Scalar,Dim> TransformType;
typedef typename TransformType::LinearPart MatrixType;
typedef const CwiseUnaryOp<
ei_scalar_multiple_op<Scalar>,
NestByValue<CwiseBinaryOp<
ei_scalar_sum_op<Scalar>,
NestByValue<typename ProductReturnType<NestByValue<MatrixType>,Other>::Type >,
NestByValue<typename TransformType::TranslationPart> > >
> ResultType;
// FIXME should we offer an optimized version when the last row is known to be 0,0...,0,1 ?
static ResultType run(const TransformType& tr, const Other& other)
{ return ((tr.linear().nestByValue() * other).nestByValue() + tr.translation().nestByValue()).nestByValue()
* (Scalar(1) / ( (tr.matrix().template block<1,Dim>(Dim,0) * other).coeff(0) + tr.matrix().coeff(Dim,Dim))); }
};
#endif // EIGEN_TRANSFORM_H

198
ground/openpilotgcs/src/libs/eigen/Eigen/src/Geometry/Translation.h Normal file
View File

@ -0,0 +1,198 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_TRANSLATION_H
#define EIGEN_TRANSLATION_H
/** \geometry_module \ingroup Geometry_Module
*
* \class Translation
*
* \brief Represents a translation transformation
*
* \param _Scalar the scalar type, i.e., the type of the coefficients.
* \param _Dim the dimension of the space, can be a compile time value or Dynamic
*
* \note This class is not aimed to be used to store a translation transformation,
* but rather to make easier the constructions and updates of Transform objects.
*
* \sa class Scaling, class Transform
*/
template<typename _Scalar, int _Dim>
class Translation
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_Dim)
/** dimension of the space */
enum { Dim = _Dim };
/** the scalar type of the coefficients */
typedef _Scalar Scalar;
/** corresponding vector type */
typedef Matrix<Scalar,Dim,1> VectorType;
/** corresponding linear transformation matrix type */
typedef Matrix<Scalar,Dim,Dim> LinearMatrixType;
/** corresponding scaling transformation type */
typedef Scaling<Scalar,Dim> ScalingType;
/** corresponding affine transformation type */
typedef Transform<Scalar,Dim> TransformType;
protected:
VectorType m_coeffs;
public:
/** Default constructor without initialization. */
Translation() {}
/** */
inline Translation(const Scalar& sx, const Scalar& sy)
{
ei_assert(Dim==2);
m_coeffs.x() = sx;
m_coeffs.y() = sy;
}
/** */
inline Translation(const Scalar& sx, const Scalar& sy, const Scalar& sz)
{
ei_assert(Dim==3);
m_coeffs.x() = sx;
m_coeffs.y() = sy;
m_coeffs.z() = sz;
}
/** Constructs and initialize the scaling transformation from a vector of scaling coefficients */
explicit inline Translation(const VectorType& vector) : m_coeffs(vector) {}
const VectorType& vector() const { return m_coeffs; }
VectorType& vector() { return m_coeffs; }
/** Concatenates two translation */
inline Translation operator* (const Translation& other) const
{ return Translation(m_coeffs + other.m_coeffs); }
/** Concatenates a translation and a scaling */
inline TransformType operator* (const ScalingType& other) const;
/** Concatenates a translation and a linear transformation */
inline TransformType operator* (const LinearMatrixType& linear) const;
template<typename Derived>
inline TransformType operator*(const RotationBase<Derived,Dim>& r) const
{ return *this * r.toRotationMatrix(); }
/** Concatenates a linear transformation and a translation */
// its a nightmare to define a templated friend function outside its declaration
friend inline TransformType operator* (const LinearMatrixType& linear, const Translation& t)
{
TransformType res;
res.matrix().setZero();
res.linear() = linear;
res.translation() = linear * t.m_coeffs;
res.matrix().row(Dim).setZero();
res(Dim,Dim) = Scalar(1);
return res;
}
/** Concatenates a translation and an affine transformation */
inline TransformType operator* (const TransformType& t) const;
/** Applies translation to vector */
inline VectorType operator* (const VectorType& other) const
{ return m_coeffs + other; }
/** \returns the inverse translation (opposite) */
Translation inverse() const { return Translation(-m_coeffs); }
Translation& operator=(const Translation& other)
{
m_coeffs = other.m_coeffs;
return *this;
}
/** \returns \c *this with scalar type casted to \a NewScalarType
*
* Note that if \a NewScalarType is equal to the current scalar type of \c *this
* then this function smartly returns a const reference to \c *this.
*/
template<typename NewScalarType>
inline typename ei_cast_return_type<Translation,Translation<NewScalarType,Dim> >::type cast() const
{ return typename ei_cast_return_type<Translation,Translation<NewScalarType,Dim> >::type(*this); }
/** Copy constructor with scalar type conversion */
template<typename OtherScalarType>
inline explicit Translation(const Translation<OtherScalarType,Dim>& other)
{ m_coeffs = other.vector().template cast<Scalar>(); }
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
bool isApprox(const Translation& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
{ return m_coeffs.isApprox(other.m_coeffs, prec); }
};
/** \addtogroup Geometry_Module */
//@{
typedef Translation<float, 2> Translation2f;
typedef Translation<double,2> Translation2d;
typedef Translation<float, 3> Translation3f;
typedef Translation<double,3> Translation3d;
//@}
template<typename Scalar, int Dim>
inline typename Translation<Scalar,Dim>::TransformType
Translation<Scalar,Dim>::operator* (const ScalingType& other) const
{
TransformType res;
res.matrix().setZero();
res.linear().diagonal() = other.coeffs();
res.translation() = m_coeffs;
res(Dim,Dim) = Scalar(1);
return res;
}
template<typename Scalar, int Dim>
inline typename Translation<Scalar,Dim>::TransformType
Translation<Scalar,Dim>::operator* (const LinearMatrixType& linear) const
{
TransformType res;
res.matrix().setZero();
res.linear() = linear;
res.translation() = m_coeffs;
res.matrix().row(Dim).setZero();
res(Dim,Dim) = Scalar(1);
return res;
}
template<typename Scalar, int Dim>
inline typename Translation<Scalar,Dim>::TransformType
Translation<Scalar,Dim>::operator* (const TransformType& t) const
{
TransformType res = t;
res.pretranslate(m_coeffs);
return res;
}
#endif // EIGEN_TRANSLATION_H

6
ground/openpilotgcs/src/libs/eigen/Eigen/src/LU/CMakeLists.txt Normal file
View File

@ -0,0 +1,6 @@
FILE(GLOB Eigen_LU_SRCS "*.h")
INSTALL(FILES
${Eigen_LU_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/LU
)

122
ground/openpilotgcs/src/libs/eigen/Eigen/src/LU/Determinant.h Normal file
View File

@ -0,0 +1,122 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_DETERMINANT_H
#define EIGEN_DETERMINANT_H
template<typename Derived>
inline const typename Derived::Scalar ei_bruteforce_det3_helper
(const MatrixBase<Derived>& matrix, int a, int b, int c)
{
return matrix.coeff(0,a)
* (matrix.coeff(1,b) * matrix.coeff(2,c) - matrix.coeff(1,c) * matrix.coeff(2,b));
}
template<typename Derived>
const typename Derived::Scalar ei_bruteforce_det4_helper
(const MatrixBase<Derived>& matrix, int j, int k, int m, int n)
{
return (matrix.coeff(j,0) * matrix.coeff(k,1) - matrix.coeff(k,0) * matrix.coeff(j,1))
* (matrix.coeff(m,2) * matrix.coeff(n,3) - matrix.coeff(n,2) * matrix.coeff(m,3));
}
const int TriangularDeterminant = 0;
template<typename Derived,
int DeterminantType =
(Derived::Flags & (UpperTriangularBit | LowerTriangularBit))
? TriangularDeterminant : Derived::RowsAtCompileTime
> struct ei_determinant_impl
{
static inline typename ei_traits<Derived>::Scalar run(const Derived& m)
{
return m.lu().determinant();
}
};
template<typename Derived> struct ei_determinant_impl<Derived, TriangularDeterminant>
{
static inline typename ei_traits<Derived>::Scalar run(const Derived& m)
{
if (Derived::Flags & UnitDiagBit)
return 1;
else if (Derived::Flags & ZeroDiagBit)
return 0;
else
return m.diagonal().redux(ei_scalar_product_op<typename ei_traits<Derived>::Scalar>());
}
};
template<typename Derived> struct ei_determinant_impl<Derived, 1>
{
static inline typename ei_traits<Derived>::Scalar run(const Derived& m)
{
return m.coeff(0,0);
}
};
template<typename Derived> struct ei_determinant_impl<Derived, 2>
{
static inline typename ei_traits<Derived>::Scalar run(const Derived& m)
{
return m.coeff(0,0) * m.coeff(1,1) - m.coeff(1,0) * m.coeff(0,1);
}
};
template<typename Derived> struct ei_determinant_impl<Derived, 3>
{
static typename ei_traits<Derived>::Scalar run(const Derived& m)
{
return ei_bruteforce_det3_helper(m,0,1,2)
- ei_bruteforce_det3_helper(m,1,0,2)
+ ei_bruteforce_det3_helper(m,2,0,1);
}
};
template<typename Derived> struct ei_determinant_impl<Derived, 4>
{
static typename ei_traits<Derived>::Scalar run(const Derived& m)
{
// trick by Martin Costabel to compute 4x4 det with only 30 muls
return ei_bruteforce_det4_helper(m,0,1,2,3)
- ei_bruteforce_det4_helper(m,0,2,1,3)
+ ei_bruteforce_det4_helper(m,0,3,1,2)
+ ei_bruteforce_det4_helper(m,1,2,0,3)
- ei_bruteforce_det4_helper(m,1,3,0,2)
+ ei_bruteforce_det4_helper(m,2,3,0,1);
}
};
/** \lu_module
*
* \returns the determinant of this matrix
*/
template<typename Derived>
inline typename ei_traits<Derived>::Scalar MatrixBase<Derived>::determinant() const
{
assert(rows() == cols());
return ei_determinant_impl<Derived>::run(derived());
}
#endif // EIGEN_DETERMINANT_H

323
ground/openpilotgcs/src/libs/eigen/Eigen/src/LU/Inverse.h Normal file
View File

@ -0,0 +1,323 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_INVERSE_H
#define EIGEN_INVERSE_H
/********************************************************************
*** Part 1 : optimized implementations for fixed-size 2,3,4 cases ***
********************************************************************/
template<typename XprType, typename MatrixType>
void ei_compute_inverse_in_size2_case(const XprType& matrix, MatrixType* result)
{
typedef typename MatrixType::Scalar Scalar;
const Scalar invdet = Scalar(1) / matrix.determinant();
result->coeffRef(0,0) = matrix.coeff(1,1) * invdet;
result->coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
result->coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
result->coeffRef(1,1) = matrix.coeff(0,0) * invdet;
}
template<typename XprType, typename MatrixType>
bool ei_compute_inverse_in_size2_case_with_check(const XprType& matrix, MatrixType* result)
{
typedef typename MatrixType::Scalar Scalar;
const Scalar det = matrix.determinant();
if(ei_isMuchSmallerThan(det, matrix.cwise().abs().maxCoeff())) return false;
const Scalar invdet = Scalar(1) / det;
result->coeffRef(0,0) = matrix.coeff(1,1) * invdet;
result->coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
result->coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
result->coeffRef(1,1) = matrix.coeff(0,0) * invdet;
return true;
}
template<typename Derived, typename OtherDerived>
void ei_compute_inverse_in_size3_case(const Derived& matrix, OtherDerived* result)
{
typedef typename Derived::Scalar Scalar;
const Scalar det_minor00 = matrix.minor(0,0).determinant();
const Scalar det_minor10 = matrix.minor(1,0).determinant();
const Scalar det_minor20 = matrix.minor(2,0).determinant();
const Scalar invdet = Scalar(1) / ( det_minor00 * matrix.coeff(0,0)
- det_minor10 * matrix.coeff(1,0)
+ det_minor20 * matrix.coeff(2,0) );
result->coeffRef(0, 0) = det_minor00 * invdet;
result->coeffRef(0, 1) = -det_minor10 * invdet;
result->coeffRef(0, 2) = det_minor20 * invdet;
result->coeffRef(1, 0) = -matrix.minor(0,1).determinant() * invdet;
result->coeffRef(1, 1) = matrix.minor(1,1).determinant() * invdet;
result->coeffRef(1, 2) = -matrix.minor(2,1).determinant() * invdet;
result->coeffRef(2, 0) = matrix.minor(0,2).determinant() * invdet;
result->coeffRef(2, 1) = -matrix.minor(1,2).determinant() * invdet;
result->coeffRef(2, 2) = matrix.minor(2,2).determinant() * invdet;
}
template<typename Derived, typename OtherDerived, typename Scalar = typename Derived::Scalar>
struct ei_compute_inverse_in_size4_case
{
static void run(const Derived& matrix, OtherDerived& result)
{
result.coeffRef(0,0) = matrix.minor(0,0).determinant();
result.coeffRef(1,0) = -matrix.minor(0,1).determinant();
result.coeffRef(2,0) = matrix.minor(0,2).determinant();
result.coeffRef(3,0) = -matrix.minor(0,3).determinant();
result.coeffRef(0,2) = matrix.minor(2,0).determinant();
result.coeffRef(1,2) = -matrix.minor(2,1).determinant();
result.coeffRef(2,2) = matrix.minor(2,2).determinant();
result.coeffRef(3,2) = -matrix.minor(2,3).determinant();
result.coeffRef(0,1) = -matrix.minor(1,0).determinant();
result.coeffRef(1,1) = matrix.minor(1,1).determinant();
result.coeffRef(2,1) = -matrix.minor(1,2).determinant();
result.coeffRef(3,1) = matrix.minor(1,3).determinant();
result.coeffRef(0,3) = -matrix.minor(3,0).determinant();
result.coeffRef(1,3) = matrix.minor(3,1).determinant();
result.coeffRef(2,3) = -matrix.minor(3,2).determinant();
result.coeffRef(3,3) = matrix.minor(3,3).determinant();
result /= (matrix.col(0).cwise()*result.row(0).transpose()).sum();
}
};
#ifdef EIGEN_VECTORIZE_SSE
// The SSE code for the 4x4 float matrix inverse in this file comes from the file
// ftp://download.intel.com/design/PentiumIII/sml/24504301.pdf
// its copyright information is:
// Copyright (C) 1999 Intel Corporation
// See page ii of that document for legal stuff. Not being lawyers, we just assume
// here that if Intel makes this document publically available, with source code
// and detailed explanations, it's because they want their CPUs to be fed with
// good code, and therefore they presumably don't mind us using it in Eigen.
template<typename Derived, typename OtherDerived>
struct ei_compute_inverse_in_size4_case<Derived, OtherDerived, float>
{
static void run(const Derived& matrix, OtherDerived& result)
{
// Variables (Streaming SIMD Extensions registers) which will contain cofactors and, later, the
// lines of the inverted matrix.
__m128 minor0, minor1, minor2, minor3;
// Variables which will contain the lines of the reference matrix and, later (after the transposition),
// the columns of the original matrix.
__m128 row0, row1, row2, row3;
// Temporary variables and the variable that will contain the matrix determinant.
__m128 det, tmp1;
// Matrix transposition
const float *src = matrix.data();
tmp1 = _mm_loadh_pi(_mm_castpd_ps(_mm_load_sd((double*)src)), (__m64*)(src+ 4));
row1 = _mm_loadh_pi(_mm_castpd_ps(_mm_load_sd((double*)(src+8))), (__m64*)(src+12));
row0 = _mm_shuffle_ps(tmp1, row1, 0x88);
row1 = _mm_shuffle_ps(row1, tmp1, 0xDD);
tmp1 = _mm_loadh_pi(_mm_castpd_ps(_mm_load_sd((double*)(src+ 2))), (__m64*)(src+ 6));
row3 = _mm_loadh_pi(_mm_castpd_ps(_mm_load_sd((double*)(src+10))), (__m64*)(src+14));
row2 = _mm_shuffle_ps(tmp1, row3, 0x88);
row3 = _mm_shuffle_ps(row3, tmp1, 0xDD);
// Cofactors calculation. Because in the process of cofactor computation some pairs in three-
// element products are repeated, it is not reasonable to load these pairs anew every time. The
// values in the registers with these pairs are formed using shuffle instruction. Cofactors are
// calculated row by row (4 elements are placed in 1 SP FP SIMD floating point register).
tmp1 = _mm_mul_ps(row2, row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor0 = _mm_mul_ps(row1, tmp1);
minor1 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0);
minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1);
minor1 = _mm_shuffle_ps(minor1, minor1, 0x4E);
// -----------------------------------------------
tmp1 = _mm_mul_ps(row1, row2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0);
minor3 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1));
minor3 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor3);
minor3 = _mm_shuffle_ps(minor3, minor3, 0x4E);
// -----------------------------------------------
tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, 0x4E), row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
row2 = _mm_shuffle_ps(row2, row2, 0x4E);
minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0);
minor2 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1));
minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2);
minor2 = _mm_shuffle_ps(minor2, minor2, 0x4E);
// -----------------------------------------------
tmp1 = _mm_mul_ps(row0, row1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2);
minor3 = _mm_sub_ps(_mm_mul_ps(row2, tmp1), minor3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2);
minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row2, tmp1));
// -----------------------------------------------
tmp1 = _mm_mul_ps(row0, row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1));
minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1);
minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1));
// -----------------------------------------------
tmp1 = _mm_mul_ps(row0, row2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1);
minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row1, tmp1));
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1));
minor3 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor3);
// Evaluation of determinant and its reciprocal value. In the original Intel document,
// 1/det was evaluated using a fast rcpps command with subsequent approximation using
// the Newton-Raphson algorithm. Here, we go for a IEEE-compliant division instead,
// so as to not compromise precision at all.
det = _mm_mul_ps(row0, minor0);
det = _mm_add_ps(_mm_shuffle_ps(det, det, 0x4E), det);
det = _mm_add_ss(_mm_shuffle_ps(det, det, 0xB1), det);
// tmp1= _mm_rcp_ss(det);
// det= _mm_sub_ss(_mm_add_ss(tmp1, tmp1), _mm_mul_ss(det, _mm_mul_ss(tmp1, tmp1)));
det = _mm_div_ss(_mm_set_ss(1.0f), det); // <--- yay, one original line not copied from Intel
det = _mm_shuffle_ps(det, det, 0x00);
// warning, Intel's variable naming is very confusing: now 'det' is 1/det !
// Multiplication of cofactors by 1/det. Storing the inverse matrix to the address in pointer src.
minor0 = _mm_mul_ps(det, minor0);
float *dst = result.data();
_mm_storel_pi((__m64*)(dst), minor0);
_mm_storeh_pi((__m64*)(dst+2), minor0);
minor1 = _mm_mul_ps(det, minor1);
_mm_storel_pi((__m64*)(dst+4), minor1);
_mm_storeh_pi((__m64*)(dst+6), minor1);
minor2 = _mm_mul_ps(det, minor2);
_mm_storel_pi((__m64*)(dst+ 8), minor2);
_mm_storeh_pi((__m64*)(dst+10), minor2);
minor3 = _mm_mul_ps(det, minor3);
_mm_storel_pi((__m64*)(dst+12), minor3);
_mm_storeh_pi((__m64*)(dst+14), minor3);
}
};
#endif
/***********************************************
*** Part 2 : selector and MatrixBase methods ***
***********************************************/
template<typename Derived, typename OtherDerived, int Size = Derived::RowsAtCompileTime>
struct ei_compute_inverse
{
static inline void run(const Derived& matrix, OtherDerived* result)
{
LU<Derived> lu(matrix);
lu.computeInverse(result);
}
};
template<typename Derived, typename OtherDerived>
struct ei_compute_inverse<Derived, OtherDerived, 1>
{
static inline void run(const Derived& matrix, OtherDerived* result)
{
typedef typename Derived::Scalar Scalar;
result->coeffRef(0,0) = Scalar(1) / matrix.coeff(0,0);
}
};
template<typename Derived, typename OtherDerived>
struct ei_compute_inverse<Derived, OtherDerived, 2>
{
static inline void run(const Derived& matrix, OtherDerived* result)
{
ei_compute_inverse_in_size2_case(matrix, result);
}
};
template<typename Derived, typename OtherDerived>
struct ei_compute_inverse<Derived, OtherDerived, 3>
{
static inline void run(const Derived& matrix, OtherDerived* result)
{
ei_compute_inverse_in_size3_case(matrix, result);
}
};
template<typename Derived, typename OtherDerived>
struct ei_compute_inverse<Derived, OtherDerived, 4>
{
static inline void run(const Derived& matrix, OtherDerived* result)
{
ei_compute_inverse_in_size4_case<Derived, OtherDerived>::run(matrix, *result);
}
};
/** \lu_module
*
* Computes the matrix inverse of this matrix.
*
* \note This matrix must be invertible, otherwise the result is undefined.
*
* \param result Pointer to the matrix in which to store the result.
*
* Example: \include MatrixBase_computeInverse.cpp
* Output: \verbinclude MatrixBase_computeInverse.out
*
* \sa inverse()
*/
template<typename Derived>
template<typename OtherDerived>
inline void MatrixBase<Derived>::computeInverse(MatrixBase<OtherDerived> *result) const
{
ei_assert(rows() == cols());
EIGEN_STATIC_ASSERT(NumTraits<Scalar>::HasFloatingPoint,NUMERIC_TYPE_MUST_BE_FLOATING_POINT)
ei_compute_inverse<PlainMatrixType, OtherDerived>::run(eval(), static_cast<OtherDerived*>(result));
}
/** \lu_module
*
* \returns the matrix inverse of this matrix.
*
* \note This matrix must be invertible, otherwise the result is undefined.
*
* \note This method returns a matrix by value, which can be inefficient. To avoid that overhead,
* use computeInverse() instead.
*
* Example: \include MatrixBase_inverse.cpp
* Output: \verbinclude MatrixBase_inverse.out
*
* \sa computeInverse()
*/
template<typename Derived>
inline const typename MatrixBase<Derived>::PlainMatrixType MatrixBase<Derived>::inverse() const
{
PlainMatrixType result(rows(), cols());
computeInverse(&result);
return result;
}
#endif // EIGEN_INVERSE_H

543
ground/openpilotgcs/src/libs/eigen/Eigen/src/LU/LU.h Normal file
View File

@ -0,0 +1,543 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_LU_H
#define EIGEN_LU_H
/** \ingroup LU_Module
*
* \class LU
*
* \brief LU decomposition of a matrix with complete pivoting, and related features
*
* \param MatrixType the type of the matrix of which we are computing the LU decomposition
*
* This class represents a LU decomposition of any matrix, with complete pivoting: the matrix A
* is decomposed as A = PLUQ where L is unit-lower-triangular, U is upper-triangular, and P and Q
* are permutation matrices. This is a rank-revealing LU decomposition. The eigenvalues (diagonal
* coefficients) of U are sorted in such a way that any zeros are at the end, so that the rank
* of A is the index of the first zero on the diagonal of U (with indices starting at 0) if any.
*
* This decomposition provides the generic approach to solving systems of linear equations, computing
* the rank, invertibility, inverse, kernel, and determinant.
*
* This LU decomposition is very stable and well tested with large matrices. Even exact rank computation
* works at sizes larger than 1000x1000. However there are use cases where the SVD decomposition is inherently
* more stable when dealing with numerically damaged input. For example, computing the kernel is more stable with
* SVD because the SVD can determine which singular values are negligible while LU has to work at the level of matrix
* coefficients that are less meaningful in this respect.
*
* The data of the LU decomposition can be directly accessed through the methods matrixLU(),
* permutationP(), permutationQ().
*
* As an exemple, here is how the original matrix can be retrieved:
* \include class_LU.cpp
* Output: \verbinclude class_LU.out
*
* \sa MatrixBase::lu(), MatrixBase::determinant(), MatrixBase::inverse(), MatrixBase::computeInverse()
*/
template<typename MatrixType> class LU
{
public:
typedef typename MatrixType::Scalar Scalar;
typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
typedef Matrix<int, 1, MatrixType::ColsAtCompileTime, MatrixType::Options, 1, MatrixType::MaxColsAtCompileTime> IntRowVectorType;
typedef Matrix<int, MatrixType::RowsAtCompileTime, 1, MatrixType::Options, MatrixType::MaxRowsAtCompileTime, 1> IntColVectorType;
typedef Matrix<Scalar, 1, MatrixType::ColsAtCompileTime, MatrixType::Options, 1, MatrixType::MaxColsAtCompileTime> RowVectorType;
typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1, MatrixType::Options, MatrixType::MaxRowsAtCompileTime, 1> ColVectorType;
enum { MaxSmallDimAtCompileTime = EIGEN_SIZE_MIN(
MatrixType::MaxColsAtCompileTime,
MatrixType::MaxRowsAtCompileTime)
};
typedef Matrix<typename MatrixType::Scalar,
MatrixType::ColsAtCompileTime, // the number of rows in the "kernel matrix" is the number of cols of the original matrix
// so that the product "matrix * kernel = zero" makes sense
Dynamic, // we don't know at compile-time the dimension of the kernel
MatrixType::Options,
MatrixType::MaxColsAtCompileTime, // see explanation for 2nd template parameter
MatrixType::MaxColsAtCompileTime // the kernel is a subspace of the domain space, whose dimension is the number
// of columns of the original matrix
> KernelResultType;
typedef Matrix<typename MatrixType::Scalar,
MatrixType::RowsAtCompileTime, // the image is a subspace of the destination space, whose dimension is the number
// of rows of the original matrix
Dynamic, // we don't know at compile time the dimension of the image (the rank)
MatrixType::Options,
MatrixType::MaxRowsAtCompileTime, // the image matrix will consist of columns from the original matrix,
MatrixType::MaxColsAtCompileTime // so it has the same number of rows and at most as many columns.
> ImageResultType;
/** Constructor.
*
* \param matrix the matrix of which to compute the LU decomposition.
*/
LU(const MatrixType& matrix);
/** \returns the LU decomposition matrix: the upper-triangular part is U, the
* unit-lower-triangular part is L (at least for square matrices; in the non-square
* case, special care is needed, see the documentation of class LU).
*
* \sa matrixL(), matrixU()
*/
inline const MatrixType& matrixLU() const
{
return m_lu;
}
/** \returns a vector of integers, whose size is the number of rows of the matrix being decomposed,
* representing the P permutation i.e. the permutation of the rows. For its precise meaning,
* see the examples given in the documentation of class LU.
*
* \sa permutationQ()
*/
inline const IntColVectorType& permutationP() const
{
return m_p;
}
/** \returns a vector of integers, whose size is the number of columns of the matrix being
* decomposed, representing the Q permutation i.e. the permutation of the columns.
* For its precise meaning, see the examples given in the documentation of class LU.
*
* \sa permutationP()
*/
inline const IntRowVectorType& permutationQ() const
{
return m_q;
}
/** Computes a basis of the kernel of the matrix, also called the null-space of the matrix.
*
* \note This method is only allowed on non-invertible matrices, as determined by
* isInvertible(). Calling it on an invertible matrix will make an assertion fail.
*
* \param result a pointer to the matrix in which to store the kernel. The columns of this
* matrix will be set to form a basis of the kernel (it will be resized
* if necessary).
*
* Example: \include LU_computeKernel.cpp
* Output: \verbinclude LU_computeKernel.out
*
* \sa kernel(), computeImage(), image()
*/
template<typename KernelMatrixType>
void computeKernel(KernelMatrixType *result) const;
/** Computes a basis of the image of the matrix, also called the column-space or range of he matrix.
*
* \note Calling this method on the zero matrix will make an assertion fail.
*
* \param result a pointer to the matrix in which to store the image. The columns of this
* matrix will be set to form a basis of the image (it will be resized
* if necessary).
*
* Example: \include LU_computeImage.cpp
* Output: \verbinclude LU_computeImage.out
*
* \sa image(), computeKernel(), kernel()
*/
template<typename ImageMatrixType>
void computeImage(ImageMatrixType *result) const;
/** \returns the kernel of the matrix, also called its null-space. The columns of the returned matrix
* will form a basis of the kernel.
*
* \note: this method is only allowed on non-invertible matrices, as determined by
* isInvertible(). Calling it on an invertible matrix will make an assertion fail.
*
* \note: this method returns a matrix by value, which induces some inefficiency.
* If you prefer to avoid this overhead, use computeKernel() instead.
*
* Example: \include LU_kernel.cpp
* Output: \verbinclude LU_kernel.out
*
* \sa computeKernel(), image()
*/
const KernelResultType kernel() const;
/** \returns the image of the matrix, also called its column-space. The columns of the returned matrix
* will form a basis of the kernel.
*
* \note: Calling this method on the zero matrix will make an assertion fail.
*
* \note: this method returns a matrix by value, which induces some inefficiency.
* If you prefer to avoid this overhead, use computeImage() instead.
*
* Example: \include LU_image.cpp
* Output: \verbinclude LU_image.out
*
* \sa computeImage(), kernel()
*/
const ImageResultType image() const;
/** This method finds a solution x to the equation Ax=b, where A is the matrix of which
* *this is the LU decomposition, if any exists.
*
* \param b the right-hand-side of the equation to solve. Can be a vector or a matrix,
* the only requirement in order for the equation to make sense is that
* b.rows()==A.rows(), where A is the matrix of which *this is the LU decomposition.
* \param result a pointer to the vector or matrix in which to store the solution, if any exists.
* Resized if necessary, so that result->rows()==A.cols() and result->cols()==b.cols().
* If no solution exists, *result is left with undefined coefficients.
*
* \returns true if any solution exists, false if no solution exists.
*
* \note If there exist more than one solution, this method will arbitrarily choose one.
* If you need a complete analysis of the space of solutions, take the one solution obtained
* by this method and add to it elements of the kernel, as determined by kernel().
*
* Example: \include LU_solve.cpp
* Output: \verbinclude LU_solve.out
*
* \sa MatrixBase::solveTriangular(), kernel(), computeKernel(), inverse(), computeInverse()
*/
template<typename OtherDerived, typename ResultType>
bool solve(const MatrixBase<OtherDerived>& b, ResultType *result) const;
/** \returns the determinant of the matrix of which
* *this is the LU decomposition. It has only linear complexity
* (that is, O(n) where n is the dimension of the square matrix)
* as the LU decomposition has already been computed.
*
* \note This is only for square matrices.
*
* \note For fixed-size matrices of size up to 4, MatrixBase::determinant() offers
* optimized paths.
*
* \warning a determinant can be very big or small, so for matrices
* of large enough dimension, there is a risk of overflow/underflow.
*
* \sa MatrixBase::determinant()
*/
typename ei_traits<MatrixType>::Scalar determinant() const;
/** \returns the rank of the matrix of which *this is the LU decomposition.
*
* \note This is computed at the time of the construction of the LU decomposition. This
* method does not perform any further computation.
*/
inline int rank() const
{
return m_rank;
}
/** \returns the dimension of the kernel of the matrix of which *this is the LU decomposition.
*
* \note Since the rank is computed at the time of the construction of the LU decomposition, this
* method almost does not perform any further computation.
*/
inline int dimensionOfKernel() const
{
return m_lu.cols() - m_rank;
}
/** \returns true if the matrix of which *this is the LU decomposition represents an injective
* linear map, i.e. has trivial kernel; false otherwise.
*
* \note Since the rank is computed at the time of the construction of the LU decomposition, this
* method almost does not perform any further computation.
*/
inline bool isInjective() const
{
return m_rank == m_lu.cols();
}
/** \returns true if the matrix of which *this is the LU decomposition represents a surjective
* linear map; false otherwise.
*
* \note Since the rank is computed at the time of the construction of the LU decomposition, this
* method almost does not perform any further computation.
*/
inline bool isSurjective() const
{
return m_rank == m_lu.rows();
}
/** \returns true if the matrix of which *this is the LU decomposition is invertible.
*
* \note Since the rank is computed at the time of the construction of the LU decomposition, this
* method almost does not perform any further computation.
*/
inline bool isInvertible() const
{
return isInjective() && isSurjective();
}
/** Computes the inverse of the matrix of which *this is the LU decomposition.
*
* \param result a pointer to the matrix into which to store the inverse. Resized if needed.
*
* \note If this matrix is not invertible, *result is left with undefined coefficients.
* Use isInvertible() to first determine whether this matrix is invertible.
*
* \sa MatrixBase::computeInverse(), inverse()
*/
template<typename ResultType>
inline void computeInverse(ResultType *result) const
{
solve(MatrixType::Identity(m_lu.rows(), m_lu.cols()), result);
}
/** \returns the inverse of the matrix of which *this is the LU decomposition.
*
* \note If this matrix is not invertible, the returned matrix has undefined coefficients.
* Use isInvertible() to first determine whether this matrix is invertible.
*
* \sa computeInverse(), MatrixBase::inverse()
*/
inline MatrixType inverse() const
{
MatrixType result;
computeInverse(&result);
return result;
}
protected:
const MatrixType& m_originalMatrix;
MatrixType m_lu;
IntColVectorType m_p;
IntRowVectorType m_q;
int m_det_pq;
int m_rank;
RealScalar m_precision;
};
template<typename MatrixType>
LU<MatrixType>::LU(const MatrixType& matrix)
: m_originalMatrix(matrix),
m_lu(matrix),
m_p(matrix.rows()),
m_q(matrix.cols())
{
const int size = matrix.diagonal().size();
const int rows = matrix.rows();
const int cols = matrix.cols();
// this formula comes from experimenting (see "LU precision tuning" thread on the list)
// and turns out to be identical to Higham's formula used already in LDLt.
m_precision = machine_epsilon<Scalar>() * size;
IntColVectorType rows_transpositions(matrix.rows());
IntRowVectorType cols_transpositions(matrix.cols());
int number_of_transpositions = 0;
RealScalar biggest = RealScalar(0);
m_rank = size;
for(int k = 0; k < size; ++k)
{
int row_of_biggest_in_corner, col_of_biggest_in_corner;
RealScalar biggest_in_corner;
biggest_in_corner = m_lu.corner(Eigen::BottomRight, rows-k, cols-k)
.cwise().abs()
.maxCoeff(&row_of_biggest_in_corner, &col_of_biggest_in_corner);
row_of_biggest_in_corner += k;
col_of_biggest_in_corner += k;
if(k==0) biggest = biggest_in_corner;
// if the corner is negligible, then we have less than full rank, and we can finish early
if(ei_isMuchSmallerThan(biggest_in_corner, biggest, m_precision))
{
m_rank = k;
for(int i = k; i < size; i++)
{
rows_transpositions.coeffRef(i) = i;
cols_transpositions.coeffRef(i) = i;
}
break;
}
rows_transpositions.coeffRef(k) = row_of_biggest_in_corner;
cols_transpositions.coeffRef(k) = col_of_biggest_in_corner;
if(k != row_of_biggest_in_corner) {
m_lu.row(k).swap(m_lu.row(row_of_biggest_in_corner));
++number_of_transpositions;
}
if(k != col_of_biggest_in_corner) {
m_lu.col(k).swap(m_lu.col(col_of_biggest_in_corner));
++number_of_transpositions;
}
if(k<rows-1)
m_lu.col(k).end(rows-k-1) /= m_lu.coeff(k,k);
if(k<size-1)
for(int col = k + 1; col < cols; ++col)
m_lu.col(col).end(rows-k-1) -= m_lu.col(k).end(rows-k-1) * m_lu.coeff(k,col);
}
for(int k = 0; k < matrix.rows(); ++k) m_p.coeffRef(k) = k;
for(int k = size-1; k >= 0; --k)
std::swap(m_p.coeffRef(k), m_p.coeffRef(rows_transpositions.coeff(k)));
for(int k = 0; k < matrix.cols(); ++k) m_q.coeffRef(k) = k;
for(int k = 0; k < size; ++k)
std::swap(m_q.coeffRef(k), m_q.coeffRef(cols_transpositions.coeff(k)));
m_det_pq = (number_of_transpositions%2) ? -1 : 1;
}
template<typename MatrixType>
typename ei_traits<MatrixType>::Scalar LU<MatrixType>::determinant() const
{
return Scalar(m_det_pq) * m_lu.diagonal().redux(ei_scalar_product_op<Scalar>());
}
template<typename MatrixType>
template<typename KernelMatrixType>
void LU<MatrixType>::computeKernel(KernelMatrixType *result) const
{
ei_assert(!isInvertible());
const int dimker = dimensionOfKernel(), cols = m_lu.cols();
result->resize(cols, dimker);
/* Let us use the following lemma:
*
* Lemma: If the matrix A has the LU decomposition PAQ = LU,
* then Ker A = Q(Ker U).
*
* Proof: trivial: just keep in mind that P, Q, L are invertible.
*/
/* Thus, all we need to do is to compute Ker U, and then apply Q.
*
* U is upper triangular, with eigenvalues sorted so that any zeros appear at the end.
* Thus, the diagonal of U ends with exactly
* m_dimKer zero's. Let us use that to construct m_dimKer linearly
* independent vectors in Ker U.
*/
Matrix<Scalar, Dynamic, Dynamic, MatrixType::Options,
MatrixType::MaxColsAtCompileTime, MatrixType::MaxColsAtCompileTime>
y(-m_lu.corner(TopRight, m_rank, dimker));
m_lu.corner(TopLeft, m_rank, m_rank)
.template marked<UpperTriangular>()
.solveTriangularInPlace(y);
for(int i = 0; i < m_rank; ++i) result->row(m_q.coeff(i)) = y.row(i);
for(int i = m_rank; i < cols; ++i) result->row(m_q.coeff(i)).setZero();
for(int k = 0; k < dimker; ++k) result->coeffRef(m_q.coeff(m_rank+k), k) = Scalar(1);
}
template<typename MatrixType>
const typename LU<MatrixType>::KernelResultType
LU<MatrixType>::kernel() const
{
KernelResultType result(m_lu.cols(), dimensionOfKernel());
computeKernel(&result);
return result;
}
template<typename MatrixType>
template<typename ImageMatrixType>
void LU<MatrixType>::computeImage(ImageMatrixType *result) const
{
ei_assert(m_rank > 0);
result->resize(m_originalMatrix.rows(), m_rank);
for(int i = 0; i < m_rank; ++i)
result->col(i) = m_originalMatrix.col(m_q.coeff(i));
}
template<typename MatrixType>
const typename LU<MatrixType>::ImageResultType
LU<MatrixType>::image() const
{
ImageResultType result(m_originalMatrix.rows(), m_rank);
computeImage(&result);
return result;
}
template<typename MatrixType>
template<typename OtherDerived, typename ResultType>
bool LU<MatrixType>::solve(
const MatrixBase<OtherDerived>& b,
ResultType *result
) const
{
/* The decomposition PAQ = LU can be rewritten as A = P^{-1} L U Q^{-1}.
* So we proceed as follows:
* Step 1: compute c = Pb.
* Step 2: replace c by the solution x to Lx = c. Exists because L is invertible.
* Step 3: replace c by the solution x to Ux = c. Check if a solution really exists.
* Step 4: result = Qc;
*/
const int rows = m_lu.rows(), cols = m_lu.cols();
ei_assert(b.rows() == rows);
const int smalldim = std::min(rows, cols);
typename OtherDerived::PlainMatrixType c(b.rows(), b.cols());
// Step 1
for(int i = 0; i < rows; ++i) c.row(m_p.coeff(i)) = b.row(i);
// Step 2
m_lu.corner(Eigen::TopLeft,smalldim,smalldim).template marked<UnitLowerTriangular>()
.solveTriangularInPlace(
c.corner(Eigen::TopLeft, smalldim, c.cols()));
if(rows>cols)
{
c.corner(Eigen::BottomLeft, rows-cols, c.cols())
-= m_lu.corner(Eigen::BottomLeft, rows-cols, cols) * c.corner(Eigen::TopLeft, cols, c.cols());
}
// Step 3
if(!isSurjective())
{
// is c is in the image of U ?
RealScalar biggest_in_c = m_rank>0 ? c.corner(TopLeft, m_rank, c.cols()).cwise().abs().maxCoeff() : RealScalar(0);
for(int col = 0; col < c.cols(); ++col)
for(int row = m_rank; row < c.rows(); ++row)
if(!ei_isMuchSmallerThan(c.coeff(row,col), biggest_in_c, m_precision))
return false;
}
if(m_rank>0)
m_lu.corner(TopLeft, m_rank, m_rank)
.template marked<UpperTriangular>()
.solveTriangularInPlace(c.corner(TopLeft, m_rank, c.cols()));
// Step 4
result->resize(m_lu.cols(), b.cols());
for(int i = 0; i < m_rank; ++i) result->row(m_q.coeff(i)) = c.row(i);
for(int i = m_rank; i < m_lu.cols(); ++i) result->row(m_q.coeff(i)).setZero();
return true;
}
/** \lu_module
*
* \return the LU decomposition of \c *this.
*
* \sa class LU
*/
template<typename Derived>
inline const LU<typename MatrixBase<Derived>::PlainMatrixType>
MatrixBase<Derived>::lu() const
{
return LU<PlainMatrixType>(eval());
}
#endif // EIGEN_LU_H

6
ground/openpilotgcs/src/libs/eigen/Eigen/src/LeastSquares/CMakeLists.txt Normal file
View File

@ -0,0 +1,6 @@
FILE(GLOB Eigen_LeastSquares_SRCS "*.h")
INSTALL(FILES
${Eigen_LeastSquares_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/LeastSquares
)

182
ground/openpilotgcs/src/libs/eigen/Eigen/src/LeastSquares/LeastSquares.h Normal file
View File

@ -0,0 +1,182 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2006-2009 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_LEASTSQUARES_H
#define EIGEN_LEASTSQUARES_H
/** \ingroup LeastSquares_Module
*
* \leastsquares_module
*
* For a set of points, this function tries to express
* one of the coords as a linear (affine) function of the other coords.
*
* This is best explained by an example. This function works in full
* generality, for points in a space of arbitrary dimension, and also over
* the complex numbers, but for this example we will work in dimension 3
* over the real numbers (doubles).
*
* So let us work with the following set of 5 points given by their
* \f$(x,y,z)\f$ coordinates:
* @code
Vector3d points[5];
points[0] = Vector3d( 3.02, 6.89, -4.32 );
points[1] = Vector3d( 2.01, 5.39, -3.79 );
points[2] = Vector3d( 2.41, 6.01, -4.01 );
points[3] = Vector3d( 2.09, 5.55, -3.86 );
points[4] = Vector3d( 2.58, 6.32, -4.10 );
* @endcode
* Suppose that we want to express the second coordinate (\f$y\f$) as a linear
* expression in \f$x\f$ and \f$z\f$, that is,
* \f[ y=ax+bz+c \f]
* for some constants \f$a,b,c\f$. Thus, we want to find the best possible
* constants \f$a,b,c\f$ so that the plane of equation \f$y=ax+bz+c\f$ fits
* best the five above points. To do that, call this function as follows:
* @code
Vector3d coeffs; // will store the coefficients a, b, c
linearRegression(
5,
&points,
&coeffs,
1 // the coord to express as a function of
// the other ones. 0 means x, 1 means y, 2 means z.
);
* @endcode
* Now the vector \a coeffs is approximately
* \f$( 0.495 , -1.927 , -2.906 )\f$.
* Thus, we get \f$a=0.495, b = -1.927, c = -2.906\f$. Let us check for
* instance how near points[0] is from the plane of equation \f$y=ax+bz+c\f$.
* Looking at the coords of points[0], we see that:
* \f[ax+bz+c = 0.495 * 3.02 + (-1.927) * (-4.32) + (-2.906) = 6.91.\f]
* On the other hand, we have \f$y=6.89\f$. We see that the values
* \f$6.91\f$ and \f$6.89\f$
* are near, so points[0] is very near the plane of equation \f$y=ax+bz+c\f$.
*
* Let's now describe precisely the parameters:
* @param numPoints the number of points
* @param points the array of pointers to the points on which to perform the linear regression
* @param result pointer to the vector in which to store the result.
This vector must be of the same type and size as the
data points. The meaning of its coords is as follows.
For brevity, let \f$n=Size\f$,
\f$r_i=result[i]\f$,
and \f$f=funcOfOthers\f$. Denote by
\f$x_0,\ldots,x_{n-1}\f$
the n coordinates in the n-dimensional space.
Then the resulting equation is:
\f[ x_f = r_0 x_0 + \cdots + r_{f-1}x_{f-1}
+ r_{f+1}x_{f+1} + \cdots + r_{n-1}x_{n-1} + r_n. \f]
* @param funcOfOthers Determines which coord to express as a function of the
others. Coords are numbered starting from 0, so that a
value of 0 means \f$x\f$, 1 means \f$y\f$,
2 means \f$z\f$, ...
*
* \sa fitHyperplane()
*/
template<typename VectorType>
void linearRegression(int numPoints,
VectorType **points,
VectorType *result,
int funcOfOthers )
{
typedef typename VectorType::Scalar Scalar;
typedef Hyperplane<Scalar, VectorType::SizeAtCompileTime> HyperplaneType;
const int size = points[0]->size();
result->resize(size);
HyperplaneType h(size);
fitHyperplane(numPoints, points, &h);
for(int i = 0; i < funcOfOthers; i++)
result->coeffRef(i) = - h.coeffs()[i] / h.coeffs()[funcOfOthers];
for(int i = funcOfOthers; i < size; i++)
result->coeffRef(i) = - h.coeffs()[i+1] / h.coeffs()[funcOfOthers];
}
/** \ingroup LeastSquares_Module
*
* \leastsquares_module
*
* This function is quite similar to linearRegression(), so we refer to the
* documentation of this function and only list here the differences.
*
* The main difference from linearRegression() is that this function doesn't
* take a \a funcOfOthers argument. Instead, it finds a general equation
* of the form
* \f[ r_0 x_0 + \cdots + r_{n-1}x_{n-1} + r_n = 0, \f]
* where \f$n=Size\f$, \f$r_i=retCoefficients[i]\f$, and we denote by
* \f$x_0,\ldots,x_{n-1}\f$ the n coordinates in the n-dimensional space.
*
* Thus, the vector \a retCoefficients has size \f$n+1\f$, which is another
* difference from linearRegression().
*
* In practice, this function performs an hyper-plane fit in a total least square sense
* via the following steps:
* 1 - center the data to the mean
* 2 - compute the covariance matrix
* 3 - pick the eigenvector corresponding to the smallest eigenvalue of the covariance matrix
* The ratio of the smallest eigenvalue and the second one gives us a hint about the relevance
* of the solution. This value is optionally returned in \a soundness.
*
* \sa linearRegression()
*/
template<typename VectorType, typename HyperplaneType>
void fitHyperplane(int numPoints,
VectorType **points,
HyperplaneType *result,
typename NumTraits<typename VectorType::Scalar>::Real* soundness = 0)
{
typedef typename VectorType::Scalar Scalar;
typedef Matrix<Scalar,VectorType::SizeAtCompileTime,VectorType::SizeAtCompileTime> CovMatrixType;
EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorType)
ei_assert(numPoints >= 1);
int size = points[0]->size();
ei_assert(size+1 == result->coeffs().size());
// compute the mean of the data
VectorType mean = VectorType::Zero(size);
for(int i = 0; i < numPoints; ++i)
mean += *(points[i]);
mean /= numPoints;
// compute the covariance matrix
CovMatrixType covMat = CovMatrixType::Zero(size, size);
VectorType remean = VectorType::Zero(size);
for(int i = 0; i < numPoints; ++i)
{
VectorType diff = (*(points[i]) - mean).conjugate();
covMat += diff * diff.adjoint();
}
// now we just have to pick the eigen vector with smallest eigen value
SelfAdjointEigenSolver<CovMatrixType> eig(covMat);
result->normal() = eig.eigenvectors().col(0);
if (soundness)
*soundness = eig.eigenvalues().coeff(0)/eig.eigenvalues().coeff(1);
// let's compute the constant coefficient such that the
// plane pass trough the mean point:
result->offset() = - (result->normal().cwise()* mean).sum();
}
#endif // EIGEN_LEASTSQUARES_H

6
ground/openpilotgcs/src/libs/eigen/Eigen/src/QR/CMakeLists.txt Normal file
View File

@ -0,0 +1,6 @@
FILE(GLOB Eigen_QR_SRCS "*.h")
INSTALL(FILES
${Eigen_QR_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/QR
)

722
ground/openpilotgcs/src/libs/eigen/Eigen/src/QR/EigenSolver.h Normal file
View File

@ -0,0 +1,722 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_EIGENSOLVER_H
#define EIGEN_EIGENSOLVER_H
/** \ingroup QR_Module
* \nonstableyet
*
* \class EigenSolver
*
* \brief Eigen values/vectors solver for non selfadjoint matrices
*
* \param MatrixType the type of the matrix of which we are computing the eigen decomposition
*
* Currently it only support real matrices.
*
* \note this code was adapted from JAMA (public domain)
*
* \sa MatrixBase::eigenvalues(), SelfAdjointEigenSolver
*/
template<typename _MatrixType> class EigenSolver
{
public:
typedef _MatrixType MatrixType;
typedef typename MatrixType::Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
typedef std::complex<RealScalar> Complex;
typedef Matrix<Complex, MatrixType::ColsAtCompileTime, 1> EigenvalueType;
typedef Matrix<Complex, MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime> EigenvectorType;
typedef Matrix<RealScalar, MatrixType::ColsAtCompileTime, 1> RealVectorType;
typedef Matrix<RealScalar, Dynamic, 1> RealVectorTypeX;
/**
* \brief Default Constructor.
*
* The default constructor is useful in cases in which the user intends to
* perform decompositions via EigenSolver::compute(const MatrixType&).
*/
EigenSolver() : m_eivec(), m_eivalues(), m_isInitialized(false) {}
EigenSolver(const MatrixType& matrix)
: m_eivec(matrix.rows(), matrix.cols()),
m_eivalues(matrix.cols()),
m_isInitialized(false)
{
compute(matrix);
}
EigenvectorType eigenvectors(void) const;
/** \returns a real matrix V of pseudo eigenvectors.
*
* Let D be the block diagonal matrix with the real eigenvalues in 1x1 blocks,
* and any complex values u+iv in 2x2 blocks [u v ; -v u]. Then, the matrices D
* and V satisfy A*V = V*D.
*
* More precisely, if the diagonal matrix of the eigen values is:\n
* \f$
* \left[ \begin{array}{cccccc}
* u+iv & & & & & \\
* & u-iv & & & & \\
* & & a+ib & & & \\
* & & & a-ib & & \\
* & & & & x & \\
* & & & & & y \\
* \end{array} \right]
* \f$ \n
* then, we have:\n
* \f$
* D =\left[ \begin{array}{cccccc}
* u & v & & & & \\
* -v & u & & & & \\
* & & a & b & & \\
* & & -b & a & & \\
* & & & & x & \\
* & & & & & y \\
* \end{array} \right]
* \f$
*
* \sa pseudoEigenvalueMatrix()
*/
const MatrixType& pseudoEigenvectors() const
{
ei_assert(m_isInitialized && "EigenSolver is not initialized.");
return m_eivec;
}
MatrixType pseudoEigenvalueMatrix() const;
/** \returns the eigenvalues as a column vector */
EigenvalueType eigenvalues() const
{
ei_assert(m_isInitialized && "EigenSolver is not initialized.");
return m_eivalues;
}
void compute(const MatrixType& matrix);
private:
void orthes(MatrixType& matH, RealVectorType& ort);
void hqr2(MatrixType& matH);
protected:
MatrixType m_eivec;
EigenvalueType m_eivalues;
bool m_isInitialized;
};
/** \returns the real block diagonal matrix D of the eigenvalues.
*
* See pseudoEigenvectors() for the details.
*/
template<typename MatrixType>
MatrixType EigenSolver<MatrixType>::pseudoEigenvalueMatrix() const
{
ei_assert(m_isInitialized && "EigenSolver is not initialized.");
int n = m_eivec.cols();
MatrixType matD = MatrixType::Zero(n,n);
for (int i=0; i<n; ++i)
{
if (ei_isMuchSmallerThan(ei_imag(m_eivalues.coeff(i)), ei_real(m_eivalues.coeff(i))))
matD.coeffRef(i,i) = ei_real(m_eivalues.coeff(i));
else
{
matD.template block<2,2>(i,i) << ei_real(m_eivalues.coeff(i)), ei_imag(m_eivalues.coeff(i)),
-ei_imag(m_eivalues.coeff(i)), ei_real(m_eivalues.coeff(i));
++i;
}
}
return matD;
}
/** \returns the normalized complex eigenvectors as a matrix of column vectors.
*
* \sa eigenvalues(), pseudoEigenvectors()
*/
template<typename MatrixType>
typename EigenSolver<MatrixType>::EigenvectorType EigenSolver<MatrixType>::eigenvectors(void) const
{
ei_assert(m_isInitialized && "EigenSolver is not initialized.");
int n = m_eivec.cols();
EigenvectorType matV(n,n);
for (int j=0; j<n; ++j)
{
if (ei_isMuchSmallerThan(ei_abs(ei_imag(m_eivalues.coeff(j))), ei_abs(ei_real(m_eivalues.coeff(j)))))
{
// we have a real eigen value
matV.col(j) = m_eivec.col(j).template cast<Complex>();
}
else
{
// we have a pair of complex eigen values
for (int i=0; i<n; ++i)
{
matV.coeffRef(i,j) = Complex(m_eivec.coeff(i,j), m_eivec.coeff(i,j+1));
matV.coeffRef(i,j+1) = Complex(m_eivec.coeff(i,j), -m_eivec.coeff(i,j+1));
}
matV.col(j).normalize();
matV.col(j+1).normalize();
++j;
}
}
return matV;
}
template<typename MatrixType>
void EigenSolver<MatrixType>::compute(const MatrixType& matrix)
{
assert(matrix.cols() == matrix.rows());
int n = matrix.cols();
m_eivalues.resize(n,1);
MatrixType matH = matrix;
RealVectorType ort(n);
// Reduce to Hessenberg form.
orthes(matH, ort);
// Reduce Hessenberg to real Schur form.
hqr2(matH);
m_isInitialized = true;
}
// Nonsymmetric reduction to Hessenberg form.
template<typename MatrixType>
void EigenSolver<MatrixType>::orthes(MatrixType& matH, RealVectorType& ort)
{
// This is derived from the Algol procedures orthes and ortran,
// by Martin and Wilkinson, Handbook for Auto. Comp.,
// Vol.ii-Linear Algebra, and the corresponding
// Fortran subroutines in EISPACK.
int n = m_eivec.cols();
int low = 0;
int high = n-1;
for (int m = low+1; m <= high-1; ++m)
{
// Scale column.
RealScalar scale = matH.block(m, m-1, high-m+1, 1).cwise().abs().sum();
if (scale != 0.0)
{
// Compute Householder transformation.
RealScalar h = 0.0;
// FIXME could be rewritten, but this one looks better wrt cache
for (int i = high; i >= m; i--)
{
ort.coeffRef(i) = matH.coeff(i,m-1)/scale;
h += ort.coeff(i) * ort.coeff(i);
}
RealScalar g = ei_sqrt(h);
if (ort.coeff(m) > 0)
g = -g;
h = h - ort.coeff(m) * g;
ort.coeffRef(m) = ort.coeff(m) - g;
// Apply Householder similarity transformation
// H = (I-u*u'/h)*H*(I-u*u')/h)
int bSize = high-m+1;
matH.block(m, m, bSize, n-m) -= ((ort.segment(m, bSize)/h)
* (ort.segment(m, bSize).transpose() * matH.block(m, m, bSize, n-m)).lazy()).lazy();
matH.block(0, m, high+1, bSize) -= ((matH.block(0, m, high+1, bSize) * ort.segment(m, bSize)).lazy()
* (ort.segment(m, bSize)/h).transpose()).lazy();
ort.coeffRef(m) = scale*ort.coeff(m);
matH.coeffRef(m,m-1) = scale*g;
}
}
// Accumulate transformations (Algol's ortran).
m_eivec.setIdentity();
for (int m = high-1; m >= low+1; m--)
{
if (matH.coeff(m,m-1) != 0.0)
{
ort.segment(m+1, high-m) = matH.col(m-1).segment(m+1, high-m);
int bSize = high-m+1;
m_eivec.block(m, m, bSize, bSize) += ( (ort.segment(m, bSize) / (matH.coeff(m,m-1) * ort.coeff(m) ) )
* (ort.segment(m, bSize).transpose() * m_eivec.block(m, m, bSize, bSize)).lazy());
}
}
}
// Complex scalar division.
template<typename Scalar>
std::complex<Scalar> cdiv(Scalar xr, Scalar xi, Scalar yr, Scalar yi)
{
Scalar r,d;
if (ei_abs(yr) > ei_abs(yi))
{
r = yi/yr;
d = yr + r*yi;
return std::complex<Scalar>((xr + r*xi)/d, (xi - r*xr)/d);
}
else
{
r = yr/yi;
d = yi + r*yr;
return std::complex<Scalar>((r*xr + xi)/d, (r*xi - xr)/d);
}
}
// Nonsymmetric reduction from Hessenberg to real Schur form.
template<typename MatrixType>
void EigenSolver<MatrixType>::hqr2(MatrixType& matH)
{
// This is derived from the Algol procedure hqr2,
// by Martin and Wilkinson, Handbook for Auto. Comp.,
// Vol.ii-Linear Algebra, and the corresponding
// Fortran subroutine in EISPACK.
// Initialize
int nn = m_eivec.cols();
int n = nn-1;
int low = 0;
int high = nn-1;
Scalar eps = ei_pow(Scalar(2),ei_is_same_type<Scalar,float>::ret ? Scalar(-23) : Scalar(-52));
Scalar exshift = 0.0;
Scalar p=0,q=0,r=0,s=0,z=0,t,w,x,y;
// Store roots isolated by balanc and compute matrix norm
// FIXME to be efficient the following would requires a triangular reduxion code
// Scalar norm = matH.upper().cwise().abs().sum() + matH.corner(BottomLeft,n,n).diagonal().cwise().abs().sum();
Scalar norm = 0.0;
for (int j = 0; j < nn; ++j)
{
// FIXME what's the purpose of the following since the condition is always false
if ((j < low) || (j > high))
{
m_eivalues.coeffRef(j) = Complex(matH.coeff(j,j), 0.0);
}
norm += matH.row(j).segment(std::max(j-1,0), nn-std::max(j-1,0)).cwise().abs().sum();
}
// Outer loop over eigenvalue index
int iter = 0;
while (n >= low)
{
// Look for single small sub-diagonal element
int l = n;
while (l > low)
{
s = ei_abs(matH.coeff(l-1,l-1)) + ei_abs(matH.coeff(l,l));
if (s == 0.0)
s = norm;
if (ei_abs(matH.coeff(l,l-1)) < eps * s)
break;
l--;
}
// Check for convergence
// One root found
if (l == n)
{
matH.coeffRef(n,n) = matH.coeff(n,n) + exshift;
m_eivalues.coeffRef(n) = Complex(matH.coeff(n,n), 0.0);
n--;
iter = 0;
}
else if (l == n-1) // Two roots found
{
w = matH.coeff(n,n-1) * matH.coeff(n-1,n);
p = (matH.coeff(n-1,n-1) - matH.coeff(n,n)) * Scalar(0.5);
q = p * p + w;
z = ei_sqrt(ei_abs(q));
matH.coeffRef(n,n) = matH.coeff(n,n) + exshift;
matH.coeffRef(n-1,n-1) = matH.coeff(n-1,n-1) + exshift;
x = matH.coeff(n,n);
// Scalar pair
if (q >= 0)
{
if (p >= 0)
z = p + z;
else
z = p - z;
m_eivalues.coeffRef(n-1) = Complex(x + z, 0.0);
m_eivalues.coeffRef(n) = Complex(z!=0.0 ? x - w / z : m_eivalues.coeff(n-1).real(), 0.0);
x = matH.coeff(n,n-1);
s = ei_abs(x) + ei_abs(z);
p = x / s;
q = z / s;
r = ei_sqrt(p * p+q * q);
p = p / r;
q = q / r;
// Row modification
for (int j = n-1; j < nn; ++j)
{
z = matH.coeff(n-1,j);
matH.coeffRef(n-1,j) = q * z + p * matH.coeff(n,j);
matH.coeffRef(n,j) = q * matH.coeff(n,j) - p * z;
}
// Column modification
for (int i = 0; i <= n; ++i)
{
z = matH.coeff(i,n-1);
matH.coeffRef(i,n-1) = q * z + p * matH.coeff(i,n);
matH.coeffRef(i,n) = q * matH.coeff(i,n) - p * z;
}
// Accumulate transformations
for (int i = low; i <= high; ++i)
{
z = m_eivec.coeff(i,n-1);
m_eivec.coeffRef(i,n-1) = q * z + p * m_eivec.coeff(i,n);
m_eivec.coeffRef(i,n) = q * m_eivec.coeff(i,n) - p * z;
}
}
else // Complex pair
{
m_eivalues.coeffRef(n-1) = Complex(x + p, z);
m_eivalues.coeffRef(n) = Complex(x + p, -z);
}
n = n - 2;
iter = 0;
}
else // No convergence yet
{
// Form shift
x = matH.coeff(n,n);
y = 0.0;
w = 0.0;
if (l < n)
{
y = matH.coeff(n-1,n-1);
w = matH.coeff(n,n-1) * matH.coeff(n-1,n);
}
// Wilkinson's original ad hoc shift
if (iter == 10)
{
exshift += x;
for (int i = low; i <= n; ++i)
matH.coeffRef(i,i) -= x;
s = ei_abs(matH.coeff(n,n-1)) + ei_abs(matH.coeff(n-1,n-2));
x = y = Scalar(0.75) * s;
w = Scalar(-0.4375) * s * s;
}
// MATLAB's new ad hoc shift
if (iter == 30)
{
s = Scalar((y - x) / 2.0);
s = s * s + w;
if (s > 0)
{
s = ei_sqrt(s);
if (y < x)
s = -s;
s = Scalar(x - w / ((y - x) / 2.0 + s));
for (int i = low; i <= n; ++i)
matH.coeffRef(i,i) -= s;
exshift += s;
x = y = w = Scalar(0.964);
}
}
iter = iter + 1; // (Could check iteration count here.)
// Look for two consecutive small sub-diagonal elements
int m = n-2;
while (m >= l)
{
z = matH.coeff(m,m);
r = x - z;
s = y - z;
p = (r * s - w) / matH.coeff(m+1,m) + matH.coeff(m,m+1);
q = matH.coeff(m+1,m+1) - z - r - s;
r = matH.coeff(m+2,m+1);
s = ei_abs(p) + ei_abs(q) + ei_abs(r);
p = p / s;
q = q / s;
r = r / s;
if (m == l) {
break;
}
if (ei_abs(matH.coeff(m,m-1)) * (ei_abs(q) + ei_abs(r)) <
eps * (ei_abs(p) * (ei_abs(matH.coeff(m-1,m-1)) + ei_abs(z) +
ei_abs(matH.coeff(m+1,m+1)))))
{
break;
}
m--;
}
for (int i = m+2; i <= n; ++i)
{
matH.coeffRef(i,i-2) = 0.0;
if (i > m+2)
matH.coeffRef(i,i-3) = 0.0;
}
// Double QR step involving rows l:n and columns m:n
for (int k = m; k <= n-1; ++k)
{
int notlast = (k != n-1);
if (k != m) {
p = matH.coeff(k,k-1);
q = matH.coeff(k+1,k-1);
r = notlast ? matH.coeff(k+2,k-1) : Scalar(0);
x = ei_abs(p) + ei_abs(q) + ei_abs(r);
if (x != 0.0)
{
p = p / x;
q = q / x;
r = r / x;
}
}
if (x == 0.0)
break;
s = ei_sqrt(p * p + q * q + r * r);
if (p < 0)
s = -s;
if (s != 0)
{
if (k != m)
matH.coeffRef(k,k-1) = -s * x;
else if (l != m)
matH.coeffRef(k,k-1) = -matH.coeff(k,k-1);
p = p + s;
x = p / s;
y = q / s;
z = r / s;
q = q / p;
r = r / p;
// Row modification
for (int j = k; j < nn; ++j)
{
p = matH.coeff(k,j) + q * matH.coeff(k+1,j);
if (notlast)
{
p = p + r * matH.coeff(k+2,j);
matH.coeffRef(k+2,j) = matH.coeff(k+2,j) - p * z;
}
matH.coeffRef(k,j) = matH.coeff(k,j) - p * x;
matH.coeffRef(k+1,j) = matH.coeff(k+1,j) - p * y;
}
// Column modification
for (int i = 0; i <= std::min(n,k+3); ++i)
{
p = x * matH.coeff(i,k) + y * matH.coeff(i,k+1);
if (notlast)
{
p = p + z * matH.coeff(i,k+2);
matH.coeffRef(i,k+2) = matH.coeff(i,k+2) - p * r;
}
matH.coeffRef(i,k) = matH.coeff(i,k) - p;
matH.coeffRef(i,k+1) = matH.coeff(i,k+1) - p * q;
}
// Accumulate transformations
for (int i = low; i <= high; ++i)
{
p = x * m_eivec.coeff(i,k) + y * m_eivec.coeff(i,k+1);
if (notlast)
{
p = p + z * m_eivec.coeff(i,k+2);
m_eivec.coeffRef(i,k+2) = m_eivec.coeff(i,k+2) - p * r;
}
m_eivec.coeffRef(i,k) = m_eivec.coeff(i,k) - p;
m_eivec.coeffRef(i,k+1) = m_eivec.coeff(i,k+1) - p * q;
}
} // (s != 0)
} // k loop
} // check convergence
} // while (n >= low)
// Backsubstitute to find vectors of upper triangular form
if (norm == 0.0)
{
return;
}
for (n = nn-1; n >= 0; n--)
{
p = m_eivalues.coeff(n).real();
q = m_eivalues.coeff(n).imag();
// Scalar vector
if (q == 0)
{
int l = n;
matH.coeffRef(n,n) = 1.0;
for (int i = n-1; i >= 0; i--)
{
w = matH.coeff(i,i) - p;
r = (matH.row(i).segment(l,n-l+1) * matH.col(n).segment(l, n-l+1))(0,0);
if (m_eivalues.coeff(i).imag() < 0.0)
{
z = w;
s = r;
}
else
{
l = i;
if (m_eivalues.coeff(i).imag() == 0.0)
{
if (w != 0.0)
matH.coeffRef(i,n) = -r / w;
else
matH.coeffRef(i,n) = -r / (eps * norm);
}
else // Solve real equations
{
x = matH.coeff(i,i+1);
y = matH.coeff(i+1,i);
q = (m_eivalues.coeff(i).real() - p) * (m_eivalues.coeff(i).real() - p) + m_eivalues.coeff(i).imag() * m_eivalues.coeff(i).imag();
t = (x * s - z * r) / q;
matH.coeffRef(i,n) = t;
if (ei_abs(x) > ei_abs(z))
matH.coeffRef(i+1,n) = (-r - w * t) / x;
else
matH.coeffRef(i+1,n) = (-s - y * t) / z;
}
// Overflow control
t = ei_abs(matH.coeff(i,n));
if ((eps * t) * t > 1)
matH.col(n).end(nn-i) /= t;
}
}
}
else if (q < 0) // Complex vector
{
std::complex<Scalar> cc;
int l = n-1;
// Last vector component imaginary so matrix is triangular
if (ei_abs(matH.coeff(n,n-1)) > ei_abs(matH.coeff(n-1,n)))
{
matH.coeffRef(n-1,n-1) = q / matH.coeff(n,n-1);
matH.coeffRef(n-1,n) = -(matH.coeff(n,n) - p) / matH.coeff(n,n-1);
}
else
{
cc = cdiv<Scalar>(0.0,-matH.coeff(n-1,n),matH.coeff(n-1,n-1)-p,q);
matH.coeffRef(n-1,n-1) = ei_real(cc);
matH.coeffRef(n-1,n) = ei_imag(cc);
}
matH.coeffRef(n,n-1) = 0.0;
matH.coeffRef(n,n) = 1.0;
for (int i = n-2; i >= 0; i--)
{
Scalar ra,sa,vr,vi;
ra = (matH.block(i,l, 1, n-l+1) * matH.block(l,n-1, n-l+1, 1)).lazy()(0,0);
sa = (matH.block(i,l, 1, n-l+1) * matH.block(l,n, n-l+1, 1)).lazy()(0,0);
w = matH.coeff(i,i) - p;
if (m_eivalues.coeff(i).imag() < 0.0)
{
z = w;
r = ra;
s = sa;
}
else
{
l = i;
if (m_eivalues.coeff(i).imag() == 0)
{
cc = cdiv(-ra,-sa,w,q);
matH.coeffRef(i,n-1) = ei_real(cc);
matH.coeffRef(i,n) = ei_imag(cc);
}
else
{
// Solve complex equations
x = matH.coeff(i,i+1);
y = matH.coeff(i+1,i);
vr = (m_eivalues.coeff(i).real() - p) * (m_eivalues.coeff(i).real() - p) + m_eivalues.coeff(i).imag() * m_eivalues.coeff(i).imag() - q * q;
vi = (m_eivalues.coeff(i).real() - p) * Scalar(2) * q;
if ((vr == 0.0) && (vi == 0.0))
vr = eps * norm * (ei_abs(w) + ei_abs(q) + ei_abs(x) + ei_abs(y) + ei_abs(z));
cc= cdiv(x*r-z*ra+q*sa,x*s-z*sa-q*ra,vr,vi);
matH.coeffRef(i,n-1) = ei_real(cc);
matH.coeffRef(i,n) = ei_imag(cc);
if (ei_abs(x) > (ei_abs(z) + ei_abs(q)))
{
matH.coeffRef(i+1,n-1) = (-ra - w * matH.coeff(i,n-1) + q * matH.coeff(i,n)) / x;
matH.coeffRef(i+1,n) = (-sa - w * matH.coeff(i,n) - q * matH.coeff(i,n-1)) / x;
}
else
{
cc = cdiv(-r-y*matH.coeff(i,n-1),-s-y*matH.coeff(i,n),z,q);
matH.coeffRef(i+1,n-1) = ei_real(cc);
matH.coeffRef(i+1,n) = ei_imag(cc);
}
}
// Overflow control
t = std::max(ei_abs(matH.coeff(i,n-1)),ei_abs(matH.coeff(i,n)));
if ((eps * t) * t > 1)
matH.block(i, n-1, nn-i, 2) /= t;
}
}
}
}
// Vectors of isolated roots
for (int i = 0; i < nn; ++i)
{
// FIXME again what's the purpose of this test ?
// in this algo low==0 and high==nn-1 !!
if (i < low || i > high)
{
m_eivec.row(i).end(nn-i) = matH.row(i).end(nn-i);
}
}
// Back transformation to get eigenvectors of original matrix
int bRows = high-low+1;
for (int j = nn-1; j >= low; j--)
{
int bSize = std::min(j,high)-low+1;
m_eivec.col(j).segment(low, bRows) = (m_eivec.block(low, low, bRows, bSize) * matH.col(j).segment(low, bSize));
}
}
#endif // EIGEN_EIGENSOLVER_H

250
ground/openpilotgcs/src/libs/eigen/Eigen/src/QR/HessenbergDecomposition.h Normal file
View File

@ -0,0 +1,250 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_HESSENBERGDECOMPOSITION_H
#define EIGEN_HESSENBERGDECOMPOSITION_H
/** \ingroup QR_Module
* \nonstableyet
*
* \class HessenbergDecomposition
*
* \brief Reduces a squared matrix to an Hessemberg form
*
* \param MatrixType the type of the matrix of which we are computing the Hessenberg decomposition
*
* This class performs an Hessenberg decomposition of a matrix \f$ A \f$ such that:
* \f$ A = Q H Q^* \f$ where \f$ Q \f$ is unitary and \f$ H \f$ a Hessenberg matrix.
*
* \sa class Tridiagonalization, class Qr
*/
template<typename _MatrixType> class HessenbergDecomposition
{
public:
typedef _MatrixType MatrixType;
typedef typename MatrixType::Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
enum {
Size = MatrixType::RowsAtCompileTime,
SizeMinusOne = MatrixType::RowsAtCompileTime==Dynamic
? Dynamic
: MatrixType::RowsAtCompileTime-1
};
typedef Matrix<Scalar, SizeMinusOne, 1> CoeffVectorType;
typedef Matrix<RealScalar, Size, 1> DiagonalType;
typedef Matrix<RealScalar, SizeMinusOne, 1> SubDiagonalType;
typedef typename NestByValue<DiagonalCoeffs<MatrixType> >::RealReturnType DiagonalReturnType;
typedef typename NestByValue<DiagonalCoeffs<
NestByValue<Block<MatrixType,SizeMinusOne,SizeMinusOne> > > >::RealReturnType SubDiagonalReturnType;
/** This constructor initializes a HessenbergDecomposition object for
* further use with HessenbergDecomposition::compute()
*/
HessenbergDecomposition(int size = Size==Dynamic ? 2 : Size)
: m_matrix(size,size), m_hCoeffs(size-1)
{}
HessenbergDecomposition(const MatrixType& matrix)
: m_matrix(matrix),
m_hCoeffs(matrix.cols()-1)
{
_compute(m_matrix, m_hCoeffs);
}
/** Computes or re-compute the Hessenberg decomposition for the matrix \a matrix.
*
* This method allows to re-use the allocated data.
*/
void compute(const MatrixType& matrix)
{
m_matrix = matrix;
m_hCoeffs.resize(matrix.rows()-1,1);
_compute(m_matrix, m_hCoeffs);
}
/** \returns the householder coefficients allowing to
* reconstruct the matrix Q from the packed data.
*
* \sa packedMatrix()
*/
CoeffVectorType householderCoefficients(void) const { return m_hCoeffs; }
/** \returns the internal result of the decomposition.
*
* The returned matrix contains the following information:
* - the upper part and lower sub-diagonal represent the Hessenberg matrix H
* - the rest of the lower part contains the Householder vectors that, combined with
* Householder coefficients returned by householderCoefficients(),
* allows to reconstruct the matrix Q as follow:
* Q = H_{N-1} ... H_1 H_0
* where the matrices H are the Householder transformation:
* H_i = (I - h_i * v_i * v_i')
* where h_i == householderCoefficients()[i] and v_i is a Householder vector:
* v_i = [ 0, ..., 0, 1, M(i+2,i), ..., M(N-1,i) ]
*
* See LAPACK for further details on this packed storage.
*/
const MatrixType& packedMatrix(void) const { return m_matrix; }
MatrixType matrixQ(void) const;
MatrixType matrixH(void) const;
private:
static void _compute(MatrixType& matA, CoeffVectorType& hCoeffs);
protected:
MatrixType m_matrix;
CoeffVectorType m_hCoeffs;
};
#ifndef EIGEN_HIDE_HEAVY_CODE
/** \internal
* Performs a tridiagonal decomposition of \a matA in place.
*
* \param matA the input selfadjoint matrix
* \param hCoeffs returned Householder coefficients
*
* The result is written in the lower triangular part of \a matA.
*
* Implemented from Golub's "Matrix Computations", algorithm 8.3.1.
*
* \sa packedMatrix()
*/
template<typename MatrixType>
void HessenbergDecomposition<MatrixType>::_compute(MatrixType& matA, CoeffVectorType& hCoeffs)
{
assert(matA.rows()==matA.cols());
int n = matA.rows();
for (int i = 0; i<n-2; ++i)
{
// let's consider the vector v = i-th column starting at position i+1
// start of the householder transformation
// squared norm of the vector v skipping the first element
RealScalar v1norm2 = matA.col(i).end(n-(i+2)).squaredNorm();
if (ei_isMuchSmallerThan(v1norm2,static_cast<Scalar>(1)))
{
hCoeffs.coeffRef(i) = 0.;
}
else
{
Scalar v0 = matA.col(i).coeff(i+1);
RealScalar beta = ei_sqrt(ei_abs2(v0)+v1norm2);
if (ei_real(v0)>=0.)
beta = -beta;
matA.col(i).end(n-(i+2)) *= (Scalar(1)/(v0-beta));
matA.col(i).coeffRef(i+1) = beta;
Scalar h = (beta - v0) / beta;
// end of the householder transformation
// Apply similarity transformation to remaining columns,
// i.e., A = H' A H where H = I - h v v' and v = matA.col(i).end(n-i-1)
matA.col(i).coeffRef(i+1) = 1;
// first let's do A = H A
matA.corner(BottomRight,n-i-1,n-i-1) -= ((ei_conj(h) * matA.col(i).end(n-i-1)) *
(matA.col(i).end(n-i-1).adjoint() * matA.corner(BottomRight,n-i-1,n-i-1))).lazy();
// now let's do A = A H
matA.corner(BottomRight,n,n-i-1) -= ((matA.corner(BottomRight,n,n-i-1) * matA.col(i).end(n-i-1))
* (h * matA.col(i).end(n-i-1).adjoint())).lazy();
matA.col(i).coeffRef(i+1) = beta;
hCoeffs.coeffRef(i) = h;
}
}
if (NumTraits<Scalar>::IsComplex)
{
// Householder transformation on the remaining single scalar
int i = n-2;
Scalar v0 = matA.coeff(i+1,i);
RealScalar beta = ei_sqrt(ei_abs2(v0));
if (ei_real(v0)>=0.)
beta = -beta;
Scalar h = (beta - v0) / beta;
hCoeffs.coeffRef(i) = h;
// A = H* A
matA.corner(BottomRight,n-i-1,n-i) -= ei_conj(h) * matA.corner(BottomRight,n-i-1,n-i);
// A = A H
matA.col(n-1) -= h * matA.col(n-1);
}
else
{
hCoeffs.coeffRef(n-2) = 0;
}
}
/** reconstructs and returns the matrix Q */
template<typename MatrixType>
typename HessenbergDecomposition<MatrixType>::MatrixType
HessenbergDecomposition<MatrixType>::matrixQ(void) const
{
int n = m_matrix.rows();
MatrixType matQ = MatrixType::Identity(n,n);
for (int i = n-2; i>=0; i--)
{
Scalar tmp = m_matrix.coeff(i+1,i);
m_matrix.const_cast_derived().coeffRef(i+1,i) = 1;
matQ.corner(BottomRight,n-i-1,n-i-1) -=
((m_hCoeffs.coeff(i) * m_matrix.col(i).end(n-i-1)) *
(m_matrix.col(i).end(n-i-1).adjoint() * matQ.corner(BottomRight,n-i-1,n-i-1)).lazy()).lazy();
m_matrix.const_cast_derived().coeffRef(i+1,i) = tmp;
}
return matQ;
}
#endif // EIGEN_HIDE_HEAVY_CODE
/** constructs and returns the matrix H.
* Note that the matrix H is equivalent to the upper part of the packed matrix
* (including the lower sub-diagonal). Therefore, it might be often sufficient
* to directly use the packed matrix instead of creating a new one.
*/
template<typename MatrixType>
typename HessenbergDecomposition<MatrixType>::MatrixType
HessenbergDecomposition<MatrixType>::matrixH(void) const
{
// FIXME should this function (and other similar) rather take a matrix as argument
// and fill it (to avoid temporaries)
int n = m_matrix.rows();
MatrixType matH = m_matrix;
if (n>2)
matH.corner(BottomLeft,n-2, n-2).template part<LowerTriangular>().setZero();
return matH;
}
#endif // EIGEN_HESSENBERGDECOMPOSITION_H

340
ground/openpilotgcs/src/libs/eigen/Eigen/src/QR/QR.h Normal file
View File

@ -0,0 +1,340 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_QR_H
#define EIGEN_QR_H
/** \ingroup QR_Module
* \nonstableyet
*
* \class QR
*
* \brief QR decomposition of a matrix
*
* \param MatrixType the type of the matrix of which we are computing the QR decomposition
*
* This class performs a QR decomposition using Householder transformations. The result is
* stored in a compact way compatible with LAPACK.
*
* \sa MatrixBase::qr()
*/
template<typename MatrixType> class QR
{
public:
typedef typename MatrixType::Scalar Scalar;
typedef typename MatrixType::RealScalar RealScalar;
typedef Block<MatrixType, MatrixType::ColsAtCompileTime, MatrixType::ColsAtCompileTime> MatrixRBlockType;
typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, MatrixType::ColsAtCompileTime> MatrixTypeR;
typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> VectorType;
/**
* \brief Default Constructor.
*
* The default constructor is useful in cases in which the user intends to
* perform decompositions via QR::compute(const MatrixType&).
*/
QR() : m_qr(), m_hCoeffs(), m_isInitialized(false) {}
QR(const MatrixType& matrix)
: m_qr(matrix.rows(), matrix.cols()),
m_hCoeffs(matrix.cols()),
m_isInitialized(false)
{
compute(matrix);
}
/** \deprecated use isInjective()
* \returns whether or not the matrix is of full rank
*
* \note Since the rank is computed only once, i.e. the first time it is needed, this
* method almost does not perform any further computation.
*/
EIGEN_DEPRECATED bool isFullRank() const
{
ei_assert(m_isInitialized && "QR is not initialized.");
return rank() == m_qr.cols();
}
/** \returns the rank of the matrix of which *this is the QR decomposition.
*
* \note Since the rank is computed only once, i.e. the first time it is needed, this
* method almost does not perform any further computation.
*/
int rank() const;
/** \returns the dimension of the kernel of the matrix of which *this is the QR decomposition.
*
* \note Since the rank is computed only once, i.e. the first time it is needed, this
* method almost does not perform any further computation.
*/
inline int dimensionOfKernel() const
{
ei_assert(m_isInitialized && "QR is not initialized.");
return m_qr.cols() - rank();
}
/** \returns true if the matrix of which *this is the QR decomposition represents an injective
* linear map, i.e. has trivial kernel; false otherwise.
*
* \note Since the rank is computed only once, i.e. the first time it is needed, this
* method almost does not perform any further computation.
*/
inline bool isInjective() const
{
ei_assert(m_isInitialized && "QR is not initialized.");
return rank() == m_qr.cols();
}
/** \returns true if the matrix of which *this is the QR decomposition represents a surjective
* linear map; false otherwise.
*
* \note Since the rank is computed only once, i.e. the first time it is needed, this
* method almost does not perform any further computation.
*/
inline bool isSurjective() const
{
ei_assert(m_isInitialized && "QR is not initialized.");
return rank() == m_qr.rows();
}
/** \returns true if the matrix of which *this is the QR decomposition is invertible.
*
* \note Since the rank is computed only once, i.e. the first time it is needed, this
* method almost does not perform any further computation.
*/
inline bool isInvertible() const
{
ei_assert(m_isInitialized && "QR is not initialized.");
return isInjective() && isSurjective();
}
/** \returns a read-only expression of the matrix R of the actual the QR decomposition */
const Part<NestByValue<MatrixRBlockType>, UpperTriangular>
matrixR(void) const
{
ei_assert(m_isInitialized && "QR is not initialized.");
int cols = m_qr.cols();
return MatrixRBlockType(m_qr, 0, 0, cols, cols).nestByValue().template part<UpperTriangular>();
}
/** This method finds a solution x to the equation Ax=b, where A is the matrix of which
* *this is the QR decomposition, if any exists.
*
* \param b the right-hand-side of the equation to solve.
*
* \param result a pointer to the vector/matrix in which to store the solution, if any exists.
* Resized if necessary, so that result->rows()==A.cols() and result->cols()==b.cols().
* If no solution exists, *result is left with undefined coefficients.
*
* \returns true if any solution exists, false if no solution exists.
*
* \note If there exist more than one solution, this method will arbitrarily choose one.
* If you need a complete analysis of the space of solutions, take the one solution obtained
* by this method and add to it elements of the kernel, as determined by kernel().
*
* \note The case where b is a matrix is not yet implemented. Also, this
* code is space inefficient.
*
* Example: \include QR_solve.cpp
* Output: \verbinclude QR_solve.out
*
* \sa MatrixBase::solveTriangular(), kernel(), computeKernel(), inverse(), computeInverse()
*/
template<typename OtherDerived, typename ResultType>
bool solve(const MatrixBase<OtherDerived>& b, ResultType *result) const;
MatrixType matrixQ(void) const;
void compute(const MatrixType& matrix);
protected:
MatrixType m_qr;
VectorType m_hCoeffs;
mutable int m_rank;
mutable bool m_rankIsUptodate;
bool m_isInitialized;
};
/** \returns the rank of the matrix of which *this is the QR decomposition. */
template<typename MatrixType>
int QR<MatrixType>::rank() const
{
ei_assert(m_isInitialized && "QR is not initialized.");
if (!m_rankIsUptodate)
{
RealScalar maxCoeff = m_qr.diagonal().cwise().abs().maxCoeff();
int n = m_qr.cols();
m_rank = 0;
while(m_rank<n && !ei_isMuchSmallerThan(m_qr.diagonal().coeff(m_rank), maxCoeff))
++m_rank;
m_rankIsUptodate = true;
}
return m_rank;
}
#ifndef EIGEN_HIDE_HEAVY_CODE
template<typename MatrixType>
void QR<MatrixType>::compute(const MatrixType& matrix)
{
m_rankIsUptodate = false;
m_qr = matrix;
m_hCoeffs.resize(matrix.cols());
int rows = matrix.rows();
int cols = matrix.cols();
RealScalar eps2 = precision<RealScalar>()*precision<RealScalar>();
for (int k = 0; k < cols; ++k)
{
int remainingSize = rows-k;
RealScalar beta;
Scalar v0 = m_qr.col(k).coeff(k);
if (remainingSize==1)
{
if (NumTraits<Scalar>::IsComplex)
{
// Householder transformation on the remaining single scalar
beta = ei_abs(v0);
if (ei_real(v0)>0)
beta = -beta;
m_qr.coeffRef(k,k) = beta;
m_hCoeffs.coeffRef(k) = (beta - v0) / beta;
}
else
{
m_hCoeffs.coeffRef(k) = 0;
}
}
else if ((beta=m_qr.col(k).end(remainingSize-1).squaredNorm())>eps2)
// FIXME what about ei_imag(v0) ??
{
// form k-th Householder vector
beta = ei_sqrt(ei_abs2(v0)+beta);
if (ei_real(v0)>=0.)
beta = -beta;
m_qr.col(k).end(remainingSize-1) /= v0-beta;
m_qr.coeffRef(k,k) = beta;
Scalar h = m_hCoeffs.coeffRef(k) = (beta - v0) / beta;
// apply the Householder transformation (I - h v v') to remaining columns, i.e.,
// R <- (I - h v v') * R where v = [1,m_qr(k+1,k), m_qr(k+2,k), ...]
int remainingCols = cols - k -1;
if (remainingCols>0)
{
m_qr.coeffRef(k,k) = Scalar(1);
m_qr.corner(BottomRight, remainingSize, remainingCols) -= ei_conj(h) * m_qr.col(k).end(remainingSize)
* (m_qr.col(k).end(remainingSize).adjoint() * m_qr.corner(BottomRight, remainingSize, remainingCols));
m_qr.coeffRef(k,k) = beta;
}
}
else
{
m_hCoeffs.coeffRef(k) = 0;
}
}
m_isInitialized = true;
}
template<typename MatrixType>
template<typename OtherDerived, typename ResultType>
bool QR<MatrixType>::solve(
const MatrixBase<OtherDerived>& b,
ResultType *result
) const
{
ei_assert(m_isInitialized && "QR is not initialized.");
const int rows = m_qr.rows();
ei_assert(b.rows() == rows);
// enforce the computation of the rank
rank();
result->resize(m_qr.cols(), b.cols());
// TODO(keir): There is almost certainly a faster way to multiply by
// Q^T without explicitly forming matrixQ(). Investigate.
*result = matrixQ().transpose()*b;
if(m_rank==0)
return result->isZero();
if(!isSurjective())
{
// is result is in the image of R ?
RealScalar biggest_in_res = result->corner(TopLeft, m_rank, result->cols()).cwise().abs().maxCoeff();
for(int col = 0; col < result->cols(); ++col)
for(int row = m_rank; row < result->rows(); ++row)
if(!ei_isMuchSmallerThan(result->coeff(row,col), biggest_in_res))
return false;
}
m_qr.corner(TopLeft, m_rank, m_rank)
.template marked<UpperTriangular>()
.solveTriangularInPlace(result->corner(TopLeft, m_rank, result->cols()));
return true;
}
/** \returns the matrix Q */
template<typename MatrixType>
MatrixType QR<MatrixType>::matrixQ() const
{
ei_assert(m_isInitialized && "QR is not initialized.");
// compute the product Q_0 Q_1 ... Q_n-1,
// where Q_k is the k-th Householder transformation I - h_k v_k v_k'
// and v_k is the k-th Householder vector [1,m_qr(k+1,k), m_qr(k+2,k), ...]
int rows = m_qr.rows();
int cols = m_qr.cols();
MatrixType res = MatrixType::Identity(rows, cols);
for (int k = cols-1; k >= 0; k--)
{
// to make easier the computation of the transformation, let's temporarily
// overwrite m_qr(k,k) such that the end of m_qr.col(k) is exactly our Householder vector.
Scalar beta = m_qr.coeff(k,k);
m_qr.const_cast_derived().coeffRef(k,k) = 1;
int endLength = rows-k;
res.corner(BottomRight,endLength, cols-k) -= ((m_hCoeffs.coeff(k) * m_qr.col(k).end(endLength))
* (m_qr.col(k).end(endLength).adjoint() * res.corner(BottomRight,endLength, cols-k)).lazy()).lazy();
m_qr.const_cast_derived().coeffRef(k,k) = beta;
}
return res;
}
#endif // EIGEN_HIDE_HEAVY_CODE
/** \return the QR decomposition of \c *this.
*
* \sa class QR
*/
template<typename Derived>
const QR<typename MatrixBase<Derived>::PlainMatrixType>
MatrixBase<Derived>::qr() const
{
return QR<PlainMatrixType>(eval());
}
#endif // EIGEN_QR_H

43
ground/openpilotgcs/src/libs/eigen/Eigen/src/QR/QrInstantiations.cpp Normal file
View File

@ -0,0 +1,43 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_EXTERN_INSTANTIATIONS
#define EIGEN_EXTERN_INSTANTIATIONS
#endif
#include "../../Core"
#undef EIGEN_EXTERN_INSTANTIATIONS
#include "../../QR"
namespace Eigen
{
template static void ei_tridiagonal_qr_step(float* , float* , int, int, float* , int);
template static void ei_tridiagonal_qr_step(double* , double* , int, int, double* , int);
template static void ei_tridiagonal_qr_step(float* , float* , int, int, std::complex<float>* , int);
template static void ei_tridiagonal_qr_step(double* , double* , int, int, std::complex<double>* , int);
EIGEN_QR_MODULE_INSTANTIATE();
}

402
ground/openpilotgcs/src/libs/eigen/Eigen/src/QR/SelfAdjointEigenSolver.h Normal file
View File

@ -0,0 +1,402 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_SELFADJOINTEIGENSOLVER_H
#define EIGEN_SELFADJOINTEIGENSOLVER_H
/** \qr_module \ingroup QR_Module
* \nonstableyet
*
* \class SelfAdjointEigenSolver
*
* \brief Eigen values/vectors solver for selfadjoint matrix
*
* \param MatrixType the type of the matrix of which we are computing the eigen decomposition
*
* \note MatrixType must be an actual Matrix type, it can't be an expression type.
*
* \sa MatrixBase::eigenvalues(), class EigenSolver
*/
template<typename _MatrixType> class SelfAdjointEigenSolver
{
public:
enum {Size = _MatrixType::RowsAtCompileTime };
typedef _MatrixType MatrixType;
typedef typename MatrixType::Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
typedef std::complex<RealScalar> Complex;
typedef Matrix<RealScalar, MatrixType::ColsAtCompileTime, 1> RealVectorType;
typedef Matrix<RealScalar, Dynamic, 1> RealVectorTypeX;
typedef Tridiagonalization<MatrixType> TridiagonalizationType;
SelfAdjointEigenSolver()
: m_eivec(int(Size), int(Size)),
m_eivalues(int(Size))
{
ei_assert(Size!=Dynamic);
}
SelfAdjointEigenSolver(int size)
: m_eivec(size, size),
m_eivalues(size)
{}
/** Constructors computing the eigenvalues of the selfadjoint matrix \a matrix,
* as well as the eigenvectors if \a computeEigenvectors is true.
*
* \sa compute(MatrixType,bool), SelfAdjointEigenSolver(MatrixType,MatrixType,bool)
*/
SelfAdjointEigenSolver(const MatrixType& matrix, bool computeEigenvectors = true)
: m_eivec(matrix.rows(), matrix.cols()),
m_eivalues(matrix.cols())
{
compute(matrix, computeEigenvectors);
}
/** Constructors computing the eigenvalues of the generalized eigen problem
* \f$ Ax = lambda B x \f$ with \a matA the selfadjoint matrix \f$ A \f$
* and \a matB the positive definite matrix \f$ B \f$ . The eigenvectors
* are computed if \a computeEigenvectors is true.
*
* \sa compute(MatrixType,MatrixType,bool), SelfAdjointEigenSolver(MatrixType,bool)
*/
SelfAdjointEigenSolver(const MatrixType& matA, const MatrixType& matB, bool computeEigenvectors = true)
: m_eivec(matA.rows(), matA.cols()),
m_eivalues(matA.cols())
{
compute(matA, matB, computeEigenvectors);
}
void compute(const MatrixType& matrix, bool computeEigenvectors = true);
void compute(const MatrixType& matA, const MatrixType& matB, bool computeEigenvectors = true);
/** \returns the computed eigen vectors as a matrix of column vectors */
MatrixType eigenvectors(void) const
{
#ifndef NDEBUG
ei_assert(m_eigenvectorsOk);
#endif
return m_eivec;
}
/** \returns the computed eigen values */
RealVectorType eigenvalues(void) const { return m_eivalues; }
/** \returns the positive square root of the matrix
*
* \note the matrix itself must be positive in order for this to make sense.
*/
MatrixType operatorSqrt() const
{
return m_eivec * m_eivalues.cwise().sqrt().asDiagonal() * m_eivec.adjoint();
}
/** \returns the positive inverse square root of the matrix
*
* \note the matrix itself must be positive definite in order for this to make sense.
*/
MatrixType operatorInverseSqrt() const
{
return m_eivec * m_eivalues.cwise().inverse().cwise().sqrt().asDiagonal() * m_eivec.adjoint();
}
protected:
MatrixType m_eivec;
RealVectorType m_eivalues;
#ifndef NDEBUG
bool m_eigenvectorsOk;
#endif
};
#ifndef EIGEN_HIDE_HEAVY_CODE
// from Golub's "Matrix Computations", algorithm 5.1.3
template<typename Scalar>
static void ei_givens_rotation(Scalar a, Scalar b, Scalar& c, Scalar& s)
{
if (b==0)
{
c = 1; s = 0;
}
else if (ei_abs(b)>ei_abs(a))
{
Scalar t = -a/b;
s = Scalar(1)/ei_sqrt(1+t*t);
c = s * t;
}
else
{
Scalar t = -b/a;
c = Scalar(1)/ei_sqrt(1+t*t);
s = c * t;
}
}
/** \internal
*
* \qr_module
*
* Performs a QR step on a tridiagonal symmetric matrix represented as a
* pair of two vectors \a diag and \a subdiag.
*
* \param matA the input selfadjoint matrix
* \param hCoeffs returned Householder coefficients
*
* For compilation efficiency reasons, this procedure does not use eigen expression
* for its arguments.
*
* Implemented from Golub's "Matrix Computations", algorithm 8.3.2:
* "implicit symmetric QR step with Wilkinson shift"
*/
template<typename RealScalar, typename Scalar>
static void ei_tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, int start, int end, Scalar* matrixQ, int n);
/** Computes the eigenvalues of the selfadjoint matrix \a matrix,
* as well as the eigenvectors if \a computeEigenvectors is true.
*
* \sa SelfAdjointEigenSolver(MatrixType,bool), compute(MatrixType,MatrixType,bool)
*/
template<typename MatrixType>
void SelfAdjointEigenSolver<MatrixType>::compute(const MatrixType& matrix, bool computeEigenvectors)
{
#ifndef NDEBUG
m_eigenvectorsOk = computeEigenvectors;
#endif
assert(matrix.cols() == matrix.rows());
int n = matrix.cols();
m_eivalues.resize(n,1);
if(n==1)
{
m_eivalues.coeffRef(0,0) = ei_real(matrix.coeff(0,0));
m_eivec.setOnes();
return;
}
m_eivec = matrix;
// FIXME, should tridiag be a local variable of this function or an attribute of SelfAdjointEigenSolver ?
// the latter avoids multiple memory allocation when the same SelfAdjointEigenSolver is used multiple times...
// (same for diag and subdiag)
RealVectorType& diag = m_eivalues;
typename TridiagonalizationType::SubDiagonalType subdiag(n-1);
TridiagonalizationType::decomposeInPlace(m_eivec, diag, subdiag, computeEigenvectors);
int end = n-1;
int start = 0;
while (end>0)
{
for (int i = start; i<end; ++i)
if (ei_isMuchSmallerThan(ei_abs(subdiag[i]),(ei_abs(diag[i])+ei_abs(diag[i+1]))))
subdiag[i] = 0;
// find the largest unreduced block
while (end>0 && subdiag[end-1]==0)
end--;
if (end<=0)
break;
start = end - 1;
while (start>0 && subdiag[start-1]!=0)
start--;
ei_tridiagonal_qr_step(diag.data(), subdiag.data(), start, end, computeEigenvectors ? m_eivec.data() : (Scalar*)0, n);
}
// Sort eigenvalues and corresponding vectors.
// TODO make the sort optional ?
// TODO use a better sort algorithm !!
for (int i = 0; i < n-1; ++i)
{
int k;
m_eivalues.segment(i,n-i).minCoeff(&k);
if (k > 0)
{
std::swap(m_eivalues[i], m_eivalues[k+i]);
m_eivec.col(i).swap(m_eivec.col(k+i));
}
}
}
/** Computes the eigenvalues of the generalized eigen problem
* \f$ Ax = lambda B x \f$ with \a matA the selfadjoint matrix \f$ A \f$
* and \a matB the positive definite matrix \f$ B \f$ . The eigenvectors
* are computed if \a computeEigenvectors is true.
*
* \sa SelfAdjointEigenSolver(MatrixType,MatrixType,bool), compute(MatrixType,bool)
*/
template<typename MatrixType>
void SelfAdjointEigenSolver<MatrixType>::
compute(const MatrixType& matA, const MatrixType& matB, bool computeEigenvectors)
{
ei_assert(matA.cols()==matA.rows() && matB.rows()==matA.rows() && matB.cols()==matB.rows());
// Compute the cholesky decomposition of matB = L L'
LLT<MatrixType> cholB(matB);
// compute C = inv(L) A inv(L')
MatrixType matC = matA;
cholB.matrixL().solveTriangularInPlace(matC);
// FIXME since we currently do not support A * inv(L'), let's do (inv(L) A')' :
matC = matC.adjoint().eval();
cholB.matrixL().template marked<LowerTriangular>().solveTriangularInPlace(matC);
matC = matC.adjoint().eval();
// this version works too:
// matC = matC.transpose();
// cholB.matrixL().conjugate().template marked<LowerTriangular>().solveTriangularInPlace(matC);
// matC = matC.transpose();
// FIXME: this should work: (currently it only does for small matrices)
// Transpose<MatrixType> trMatC(matC);
// cholB.matrixL().conjugate().eval().template marked<LowerTriangular>().solveTriangularInPlace(trMatC);
compute(matC, computeEigenvectors);
if (computeEigenvectors)
{
// transform back the eigen vectors: evecs = inv(U) * evecs
cholB.matrixL().adjoint().template marked<UpperTriangular>().solveTriangularInPlace(m_eivec);
for (int i=0; i<m_eivec.cols(); ++i)
m_eivec.col(i) = m_eivec.col(i).normalized();
}
}
#endif // EIGEN_HIDE_HEAVY_CODE
/** \qr_module
*
* \returns a vector listing the eigenvalues of this matrix.
*/
template<typename Derived>
inline Matrix<typename NumTraits<typename ei_traits<Derived>::Scalar>::Real, ei_traits<Derived>::ColsAtCompileTime, 1>
MatrixBase<Derived>::eigenvalues() const
{
ei_assert(Flags&SelfAdjointBit);
return SelfAdjointEigenSolver<typename Derived::PlainMatrixType>(eval(),false).eigenvalues();
}
template<typename Derived, bool IsSelfAdjoint>
struct ei_operatorNorm_selector
{
static inline typename NumTraits<typename ei_traits<Derived>::Scalar>::Real
operatorNorm(const MatrixBase<Derived>& m)
{
// FIXME if it is really guaranteed that the eigenvalues are already sorted,
// then we don't need to compute a maxCoeff() here, comparing the 1st and last ones is enough.
return m.eigenvalues().cwise().abs().maxCoeff();
}
};
template<typename Derived> struct ei_operatorNorm_selector<Derived, false>
{
static inline typename NumTraits<typename ei_traits<Derived>::Scalar>::Real
operatorNorm(const MatrixBase<Derived>& m)
{
typename Derived::PlainMatrixType m_eval(m);
// FIXME if it is really guaranteed that the eigenvalues are already sorted,
// then we don't need to compute a maxCoeff() here, comparing the 1st and last ones is enough.
return ei_sqrt(
(m_eval*m_eval.adjoint())
.template marked<SelfAdjoint>()
.eigenvalues()
.maxCoeff()
);
}
};
/** \qr_module
*
* \returns the matrix norm of this matrix.
*/
template<typename Derived>
inline typename NumTraits<typename ei_traits<Derived>::Scalar>::Real
MatrixBase<Derived>::operatorNorm() const
{
return ei_operatorNorm_selector<Derived, Flags&SelfAdjointBit>
::operatorNorm(derived());
}
#ifndef EIGEN_EXTERN_INSTANTIATIONS
template<typename RealScalar, typename Scalar>
static void ei_tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, int start, int end, Scalar* matrixQ, int n)
{
RealScalar td = (diag[end-1] - diag[end])*RealScalar(0.5);
RealScalar e2 = ei_abs2(subdiag[end-1]);
RealScalar mu = diag[end] - e2 / (td + (td>0 ? 1 : -1) * ei_sqrt(td*td + e2));
RealScalar x = diag[start] - mu;
RealScalar z = subdiag[start];
for (int k = start; k < end; ++k)
{
RealScalar c, s;
ei_givens_rotation(x, z, c, s);
// do T = G' T G
RealScalar sdk = s * diag[k] + c * subdiag[k];
RealScalar dkp1 = s * subdiag[k] + c * diag[k+1];
diag[k] = c * (c * diag[k] - s * subdiag[k]) - s * (c * subdiag[k] - s * diag[k+1]);
diag[k+1] = s * sdk + c * dkp1;
subdiag[k] = c * sdk - s * dkp1;
if (k > start)
subdiag[k - 1] = c * subdiag[k-1] - s * z;
x = subdiag[k];
if (k < end - 1)
{
z = -s * subdiag[k+1];
subdiag[k + 1] = c * subdiag[k+1];
}
// apply the givens rotation to the unit matrix Q = Q * G
// G only modifies the two columns k and k+1
if (matrixQ)
{
#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
#else
int kn = k*n;
int kn1 = (k+1)*n;
#endif
// let's do the product manually to avoid the need of temporaries...
for (int i=0; i<n; ++i)
{
#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
Scalar matrixQ_i_k = matrixQ[i*n+k];
matrixQ[i*n+k] = c * matrixQ_i_k - s * matrixQ[i*n+k+1];
matrixQ[i*n+k+1] = s * matrixQ_i_k + c * matrixQ[i*n+k+1];
#else
Scalar matrixQ_i_k = matrixQ[i+kn];
matrixQ[i+kn] = c * matrixQ_i_k - s * matrixQ[i+kn1];
matrixQ[i+kn1] = s * matrixQ_i_k + c * matrixQ[i+kn1];
#endif
}
}
}
}
#endif
#endif // EIGEN_SELFADJOINTEIGENSOLVER_H

431
ground/openpilotgcs/src/libs/eigen/Eigen/src/QR/Tridiagonalization.h Normal file
View File

@ -0,0 +1,431 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_TRIDIAGONALIZATION_H
#define EIGEN_TRIDIAGONALIZATION_H
/** \ingroup QR_Module
* \nonstableyet
*
* \class Tridiagonalization
*
* \brief Trigiagonal decomposition of a selfadjoint matrix
*
* \param MatrixType the type of the matrix of which we are performing the tridiagonalization
*
* This class performs a tridiagonal decomposition of a selfadjoint matrix \f$ A \f$ such that:
* \f$ A = Q T Q^* \f$ where \f$ Q \f$ is unitary and \f$ T \f$ a real symmetric tridiagonal matrix.
*
* \sa MatrixBase::tridiagonalize()
*/
template<typename _MatrixType> class Tridiagonalization
{
public:
typedef _MatrixType MatrixType;
typedef typename MatrixType::Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
typedef typename ei_packet_traits<Scalar>::type Packet;
enum {
Size = MatrixType::RowsAtCompileTime,
SizeMinusOne = MatrixType::RowsAtCompileTime==Dynamic
? Dynamic
: MatrixType::RowsAtCompileTime-1,
PacketSize = ei_packet_traits<Scalar>::size
};
typedef Matrix<Scalar, SizeMinusOne, 1> CoeffVectorType;
typedef Matrix<RealScalar, Size, 1> DiagonalType;
typedef Matrix<RealScalar, SizeMinusOne, 1> SubDiagonalType;
typedef typename NestByValue<DiagonalCoeffs<MatrixType> >::RealReturnType DiagonalReturnType;
typedef typename NestByValue<DiagonalCoeffs<
NestByValue<Block<MatrixType,SizeMinusOne,SizeMinusOne> > > >::RealReturnType SubDiagonalReturnType;
/** This constructor initializes a Tridiagonalization object for
* further use with Tridiagonalization::compute()
*/
Tridiagonalization(int size = Size==Dynamic ? 2 : Size)
: m_matrix(size,size), m_hCoeffs(size-1)
{}
Tridiagonalization(const MatrixType& matrix)
: m_matrix(matrix),
m_hCoeffs(matrix.cols()-1)
{
_compute(m_matrix, m_hCoeffs);
}
/** Computes or re-compute the tridiagonalization for the matrix \a matrix.
*
* This method allows to re-use the allocated data.
*/
void compute(const MatrixType& matrix)
{
m_matrix = matrix;
m_hCoeffs.resize(matrix.rows()-1, 1);
_compute(m_matrix, m_hCoeffs);
}
/** \returns the householder coefficients allowing to
* reconstruct the matrix Q from the packed data.
*
* \sa packedMatrix()
*/
inline CoeffVectorType householderCoefficients(void) const { return m_hCoeffs; }
/** \returns the internal result of the decomposition.
*
* The returned matrix contains the following information:
* - the strict upper part is equal to the input matrix A
* - the diagonal and lower sub-diagonal represent the tridiagonal symmetric matrix (real).
* - the rest of the lower part contains the Householder vectors that, combined with
* Householder coefficients returned by householderCoefficients(),
* allows to reconstruct the matrix Q as follow:
* Q = H_{N-1} ... H_1 H_0
* where the matrices H are the Householder transformations:
* H_i = (I - h_i * v_i * v_i')
* where h_i == householderCoefficients()[i] and v_i is a Householder vector:
* v_i = [ 0, ..., 0, 1, M(i+2,i), ..., M(N-1,i) ]
*
* See LAPACK for further details on this packed storage.
*/
inline const MatrixType& packedMatrix(void) const { return m_matrix; }
MatrixType matrixQ(void) const;
MatrixType matrixT(void) const;
const DiagonalReturnType diagonal(void) const;
const SubDiagonalReturnType subDiagonal(void) const;
static void decomposeInPlace(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, bool extractQ = true);
private:
static void _compute(MatrixType& matA, CoeffVectorType& hCoeffs);
static void _decomposeInPlace3x3(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, bool extractQ = true);
protected:
MatrixType m_matrix;
CoeffVectorType m_hCoeffs;
};
/** \returns an expression of the diagonal vector */
template<typename MatrixType>
const typename Tridiagonalization<MatrixType>::DiagonalReturnType
Tridiagonalization<MatrixType>::diagonal(void) const
{
return m_matrix.diagonal().nestByValue().real();
}
/** \returns an expression of the sub-diagonal vector */
template<typename MatrixType>
const typename Tridiagonalization<MatrixType>::SubDiagonalReturnType
Tridiagonalization<MatrixType>::subDiagonal(void) const
{
int n = m_matrix.rows();
return Block<MatrixType,SizeMinusOne,SizeMinusOne>(m_matrix, 1, 0, n-1,n-1)
.nestByValue().diagonal().nestByValue().real();
}
/** constructs and returns the tridiagonal matrix T.
* Note that the matrix T is equivalent to the diagonal and sub-diagonal of the packed matrix.
* Therefore, it might be often sufficient to directly use the packed matrix, or the vector
* expressions returned by diagonal() and subDiagonal() instead of creating a new matrix.
*/
template<typename MatrixType>
typename Tridiagonalization<MatrixType>::MatrixType
Tridiagonalization<MatrixType>::matrixT(void) const
{
// FIXME should this function (and other similar ones) rather take a matrix as argument
// and fill it ? (to avoid temporaries)
int n = m_matrix.rows();
MatrixType matT = m_matrix;
matT.corner(TopRight,n-1, n-1).diagonal() = subDiagonal().template cast<Scalar>().conjugate();
if (n>2)
{
matT.corner(TopRight,n-2, n-2).template part<UpperTriangular>().setZero();
matT.corner(BottomLeft,n-2, n-2).template part<LowerTriangular>().setZero();
}
return matT;
}
#ifndef EIGEN_HIDE_HEAVY_CODE
/** \internal
* Performs a tridiagonal decomposition of \a matA in place.
*
* \param matA the input selfadjoint matrix
* \param hCoeffs returned Householder coefficients
*
* The result is written in the lower triangular part of \a matA.
*
* Implemented from Golub's "Matrix Computations", algorithm 8.3.1.
*
* \sa packedMatrix()
*/
template<typename MatrixType>
void Tridiagonalization<MatrixType>::_compute(MatrixType& matA, CoeffVectorType& hCoeffs)
{
assert(matA.rows()==matA.cols());
int n = matA.rows();
// std::cerr << matA << "\n\n";
for (int i = 0; i<n-2; ++i)
{
// let's consider the vector v = i-th column starting at position i+1
// start of the householder transformation
// squared norm of the vector v skipping the first element
RealScalar v1norm2 = matA.col(i).end(n-(i+2)).squaredNorm();
// FIXME comparing against 1
if (ei_isMuchSmallerThan(v1norm2,static_cast<Scalar>(1)))
{
hCoeffs.coeffRef(i) = 0.;
}
else
{
Scalar v0 = matA.col(i).coeff(i+1);
RealScalar beta = ei_sqrt(ei_abs2(v0)+v1norm2);
if (ei_real(v0)>=0.)
beta = -beta;
matA.col(i).end(n-(i+2)) *= (Scalar(1)/(v0-beta));
matA.col(i).coeffRef(i+1) = beta;
Scalar h = (beta - v0) / beta;
// end of the householder transformation
// Apply similarity transformation to remaining columns,
// i.e., A = H' A H where H = I - h v v' and v = matA.col(i).end(n-i-1)
matA.col(i).coeffRef(i+1) = 1;
/* This is the initial algorithm which minimize operation counts and maximize
* the use of Eigen's expression. Unfortunately, the first matrix-vector product
* using Part<LowerTriangular|Selfadjoint> is very very slow */
#ifdef EIGEN_NEVER_DEFINED
// matrix - vector product
hCoeffs.end(n-i-1) = (matA.corner(BottomRight,n-i-1,n-i-1).template part<LowerTriangular|SelfAdjoint>()
* (h * matA.col(i).end(n-i-1))).lazy();
// simple axpy
hCoeffs.end(n-i-1) += (h * Scalar(-0.5) * matA.col(i).end(n-i-1).dot(hCoeffs.end(n-i-1)))
* matA.col(i).end(n-i-1);
// rank-2 update
//Block<MatrixType,Dynamic,1> B(matA,i+1,i,n-i-1,1);
matA.corner(BottomRight,n-i-1,n-i-1).template part<LowerTriangular>() -=
(matA.col(i).end(n-i-1) * hCoeffs.end(n-i-1).adjoint()).lazy()
+ (hCoeffs.end(n-i-1) * matA.col(i).end(n-i-1).adjoint()).lazy();
#endif
/* end initial algorithm */
/* If we still want to minimize operation count (i.e., perform operation on the lower part only)
* then we could provide the following algorithm for selfadjoint - vector product. However, a full
* matrix-vector product is still faster (at least for dynamic size, and not too small, did not check
* small matrices). The algo performs block matrix-vector and transposed matrix vector products. */
#ifdef EIGEN_NEVER_DEFINED
int n4 = (std::max(0,n-4)/4)*4;
hCoeffs.end(n-i-1).setZero();
for (int b=i+1; b<n4; b+=4)
{
// the ?x4 part:
hCoeffs.end(b-4) +=
Block<MatrixType,Dynamic,4>(matA,b+4,b,n-b-4,4) * matA.template block<4,1>(b,i);
// the respective transposed part:
Block<CoeffVectorType,4,1>(hCoeffs, b, 0, 4,1) +=
Block<MatrixType,Dynamic,4>(matA,b+4,b,n-b-4,4).adjoint() * Block<MatrixType,Dynamic,1>(matA,b+4,i,n-b-4,1);
// the 4x4 block diagonal:
Block<CoeffVectorType,4,1>(hCoeffs, b, 0, 4,1) +=
(Block<MatrixType,4,4>(matA,b,b,4,4).template part<LowerTriangular|SelfAdjoint>()
* (h * Block<MatrixType,4,1>(matA,b,i,4,1))).lazy();
}
#endif
// todo: handle the remaining part
/* end optimized selfadjoint - vector product */
/* Another interesting note: the above rank-2 update is much slower than the following hand written loop.
* After an analyze of the ASM, it seems GCC (4.2) generate poor code because of the Block. Moreover,
* if we remove the specialization of Block for Matrix then it is even worse, much worse ! */
#ifdef EIGEN_NEVER_DEFINED
for (int j1=i+1; j1<n; ++j1)
for (int i1=j1; i1<n; ++i1)
matA.coeffRef(i1,j1) -= matA.coeff(i1,i)*ei_conj(hCoeffs.coeff(j1-1))
+ hCoeffs.coeff(i1-1)*ei_conj(matA.coeff(j1,i));
#endif
/* end hand writen partial rank-2 update */
/* The current fastest implementation: the full matrix is used, no "optimization" to use/compute
* only half of the matrix. Custom vectorization of the inner col -= alpha X + beta Y such that access
* to col are always aligned. Once we support that in Assign, then the algorithm could be rewriten as
* a single compact expression. This code is therefore a good benchmark when will do that. */
// let's use the end of hCoeffs to store temporary values:
hCoeffs.end(n-i-1) = (matA.corner(BottomRight,n-i-1,n-i-1) * (h * matA.col(i).end(n-i-1))).lazy();
// FIXME in the above expr a temporary is created because of the scalar multiple by h
hCoeffs.end(n-i-1) += (h * Scalar(-0.5) * matA.col(i).end(n-i-1).dot(hCoeffs.end(n-i-1)))
* matA.col(i).end(n-i-1);
const Scalar* EIGEN_RESTRICT pb = &matA.coeffRef(0,i);
const Scalar* EIGEN_RESTRICT pa = (&hCoeffs.coeffRef(0)) - 1;
for (int j1=i+1; j1<n; ++j1)
{
int starti = i+1;
int alignedEnd = starti;
if (PacketSize>1 && (int(MatrixType::Flags)&RowMajor) == 0)
{
int alignedStart = (starti) + ei_alignmentOffset(&matA.coeffRef(starti,j1), n-starti);
alignedEnd = alignedStart + ((n-alignedStart)/PacketSize)*PacketSize;
for (int i1=starti; i1<alignedStart; ++i1)
matA.coeffRef(i1,j1) -= matA.coeff(i1,i)*ei_conj(hCoeffs.coeff(j1-1))
+ hCoeffs.coeff(i1-1)*ei_conj(matA.coeff(j1,i));
Packet tmp0 = ei_pset1(hCoeffs.coeff(j1-1));
Packet tmp1 = ei_pset1(matA.coeff(j1,i));
Scalar* pc = &matA.coeffRef(0,j1);
for (int i1=alignedStart ; i1<alignedEnd; i1+=PacketSize)
ei_pstore(pc+i1,ei_psub(ei_pload(pc+i1),
ei_padd(ei_pmul(tmp0, ei_ploadu(pb+i1)),
ei_pmul(tmp1, ei_ploadu(pa+i1)))));
}
for (int i1=alignedEnd; i1<n; ++i1)
matA.coeffRef(i1,j1) -= matA.coeff(i1,i)*ei_conj(hCoeffs.coeff(j1-1))
+ hCoeffs.coeff(i1-1)*ei_conj(matA.coeff(j1,i));
}
/* end optimized implementation */
// note: at that point matA(i+1,i+1) is the (i+1)-th element of the final diagonal
// note: the sequence of the beta values leads to the subdiagonal entries
matA.col(i).coeffRef(i+1) = beta;
hCoeffs.coeffRef(i) = h;
}
}
if (NumTraits<Scalar>::IsComplex)
{
// Householder transformation on the remaining single scalar
int i = n-2;
Scalar v0 = matA.col(i).coeff(i+1);
RealScalar beta = ei_abs(v0);
if (ei_real(v0)>=0.)
beta = -beta;
matA.col(i).coeffRef(i+1) = beta;
if(ei_isMuchSmallerThan(beta, Scalar(1))) hCoeffs.coeffRef(i) = Scalar(0);
else hCoeffs.coeffRef(i) = (beta - v0) / beta;
}
else
{
hCoeffs.coeffRef(n-2) = 0;
}
}
/** reconstructs and returns the matrix Q */
template<typename MatrixType>
typename Tridiagonalization<MatrixType>::MatrixType
Tridiagonalization<MatrixType>::matrixQ(void) const
{
int n = m_matrix.rows();
MatrixType matQ = MatrixType::Identity(n,n);
for (int i = n-2; i>=0; i--)
{
Scalar tmp = m_matrix.coeff(i+1,i);
m_matrix.const_cast_derived().coeffRef(i+1,i) = 1;
matQ.corner(BottomRight,n-i-1,n-i-1) -=
((m_hCoeffs.coeff(i) * m_matrix.col(i).end(n-i-1)) *
(m_matrix.col(i).end(n-i-1).adjoint() * matQ.corner(BottomRight,n-i-1,n-i-1)).lazy()).lazy();
m_matrix.const_cast_derived().coeffRef(i+1,i) = tmp;
}
return matQ;
}
/** Performs a full decomposition in place */
template<typename MatrixType>
void Tridiagonalization<MatrixType>::decomposeInPlace(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, bool extractQ)
{
int n = mat.rows();
ei_assert(mat.cols()==n && diag.size()==n && subdiag.size()==n-1);
if (n==3 && (!NumTraits<Scalar>::IsComplex) )
{
_decomposeInPlace3x3(mat, diag, subdiag, extractQ);
}
else
{
Tridiagonalization tridiag(mat);
diag = tridiag.diagonal();
subdiag = tridiag.subDiagonal();
if (extractQ)
mat = tridiag.matrixQ();
}
}
/** \internal
* Optimized path for 3x3 matrices.
* Especially useful for plane fitting.
*/
template<typename MatrixType>
void Tridiagonalization<MatrixType>::_decomposeInPlace3x3(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, bool extractQ)
{
diag[0] = ei_real(mat(0,0));
RealScalar v1norm2 = ei_abs2(mat(0,2));
if (ei_isMuchSmallerThan(v1norm2, RealScalar(1)))
{
diag[1] = ei_real(mat(1,1));
diag[2] = ei_real(mat(2,2));
subdiag[0] = ei_real(mat(0,1));
subdiag[1] = ei_real(mat(1,2));
if (extractQ)
mat.setIdentity();
}
else
{
RealScalar beta = ei_sqrt(ei_abs2(mat(0,1))+v1norm2);
RealScalar invBeta = RealScalar(1)/beta;
Scalar m01 = mat(0,1) * invBeta;
Scalar m02 = mat(0,2) * invBeta;
Scalar q = RealScalar(2)*m01*mat(1,2) + m02*(mat(2,2) - mat(1,1));
diag[1] = ei_real(mat(1,1) + m02*q);
diag[2] = ei_real(mat(2,2) - m02*q);
subdiag[0] = beta;
subdiag[1] = ei_real(mat(1,2) - m01 * q);
if (extractQ)
{
mat(0,0) = 1;
mat(0,1) = 0;
mat(0,2) = 0;
mat(1,0) = 0;
mat(1,1) = m01;
mat(1,2) = m02;
mat(2,0) = 0;
mat(2,1) = m02;
mat(2,2) = -m01;
}
}
}
#endif // EIGEN_HIDE_HEAVY_CODE
#endif // EIGEN_TRIDIAGONALIZATION_H

6
ground/openpilotgcs/src/libs/eigen/Eigen/src/SVD/CMakeLists.txt Normal file
View File

@ -0,0 +1,6 @@
FILE(GLOB Eigen_SVD_SRCS "*.h")
INSTALL(FILES
${Eigen_SVD_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/SVD
)

649
ground/openpilotgcs/src/libs/eigen/Eigen/src/SVD/SVD.h Normal file
View File

@ -0,0 +1,649 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_SVD_H
#define EIGEN_SVD_H
/** \ingroup SVD_Module
* \nonstableyet
*
* \class SVD
*
* \brief Standard SVD decomposition of a matrix and associated features
*
* \param MatrixType the type of the matrix of which we are computing the SVD decomposition
*
* This class performs a standard SVD decomposition of a real matrix A of size \c M x \c N
* with \c M \>= \c N.
*
*
* \sa MatrixBase::SVD()
*/
template<typename MatrixType> class SVD
{
private:
typedef typename MatrixType::Scalar Scalar;
typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
enum {
PacketSize = ei_packet_traits<Scalar>::size,
AlignmentMask = int(PacketSize)-1,
MinSize = EIGEN_SIZE_MIN(MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime)
};
typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> ColVector;
typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> RowVector;
typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MinSize> MatrixUType;
typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, MatrixType::ColsAtCompileTime> MatrixVType;
typedef Matrix<Scalar, MinSize, 1> SingularValuesType;
public:
SVD() {} // a user who relied on compiler-generated default compiler reported problems with MSVC in 2.0.7
SVD(const MatrixType& matrix)
: m_matU(matrix.rows(), std::min(matrix.rows(), matrix.cols())),
m_matV(matrix.cols(),matrix.cols()),
m_sigma(std::min(matrix.rows(),matrix.cols()))
{
compute(matrix);
}
template<typename OtherDerived, typename ResultType>
bool solve(const MatrixBase<OtherDerived> &b, ResultType* result) const;
const MatrixUType& matrixU() const { return m_matU; }
const SingularValuesType& singularValues() const { return m_sigma; }
const MatrixVType& matrixV() const { return m_matV; }
void compute(const MatrixType& matrix);
SVD& sort();
template<typename UnitaryType, typename PositiveType>
void computeUnitaryPositive(UnitaryType *unitary, PositiveType *positive) const;
template<typename PositiveType, typename UnitaryType>
void computePositiveUnitary(PositiveType *positive, UnitaryType *unitary) const;
template<typename RotationType, typename ScalingType>
void computeRotationScaling(RotationType *unitary, ScalingType *positive) const;
template<typename ScalingType, typename RotationType>
void computeScalingRotation(ScalingType *positive, RotationType *unitary) const;
protected:
/** \internal */
MatrixUType m_matU;
/** \internal */
MatrixVType m_matV;
/** \internal */
SingularValuesType m_sigma;
};
/** Computes / recomputes the SVD decomposition A = U S V^* of \a matrix
*
* \note this code has been adapted from JAMA (public domain)
*/
template<typename MatrixType>
void SVD<MatrixType>::compute(const MatrixType& matrix)
{
const int m = matrix.rows();
const int n = matrix.cols();
const int nu = std::min(m,n);
ei_assert(m>=n && "In Eigen 2.0, SVD only works for MxN matrices with M>=N. Sorry!");
ei_assert(m>1 && "In Eigen 2.0, SVD doesn't work on 1x1 matrices");
m_matU.resize(m, nu);
m_matU.setZero();
m_sigma.resize(std::min(m,n));
m_matV.resize(n,n);
RowVector e(n);
ColVector work(m);
MatrixType matA(matrix);
const bool wantu = true;
const bool wantv = true;
int i=0, j=0, k=0;
// Reduce A to bidiagonal form, storing the diagonal elements
// in s and the super-diagonal elements in e.
int nct = std::min(m-1,n);
int nrt = std::max(0,std::min(n-2,m));
for (k = 0; k < std::max(nct,nrt); ++k)
{
if (k < nct)
{
// Compute the transformation for the k-th column and
// place the k-th diagonal in m_sigma[k].
m_sigma[k] = matA.col(k).end(m-k).norm();
if (m_sigma[k] != 0.0) // FIXME
{
if (matA(k,k) < 0.0)
m_sigma[k] = -m_sigma[k];
matA.col(k).end(m-k) /= m_sigma[k];
matA(k,k) += 1.0;
}
m_sigma[k] = -m_sigma[k];
}
for (j = k+1; j < n; ++j)
{
if ((k < nct) && (m_sigma[k] != 0.0))
{
// Apply the transformation.
Scalar t = matA.col(k).end(m-k).dot(matA.col(j).end(m-k)); // FIXME dot product or cwise prod + .sum() ??
t = -t/matA(k,k);
matA.col(j).end(m-k) += t * matA.col(k).end(m-k);
}
// Place the k-th row of A into e for the
// subsequent calculation of the row transformation.
e[j] = matA(k,j);
}
// Place the transformation in U for subsequent back multiplication.
if (wantu & (k < nct))
m_matU.col(k).end(m-k) = matA.col(k).end(m-k);
if (k < nrt)
{
// Compute the k-th row transformation and place the
// k-th super-diagonal in e[k].
e[k] = e.end(n-k-1).norm();
if (e[k] != 0.0)
{
if (e[k+1] < 0.0)
e[k] = -e[k];
e.end(n-k-1) /= e[k];
e[k+1] += 1.0;
}
e[k] = -e[k];
if ((k+1 < m) & (e[k] != 0.0))
{
// Apply the transformation.
work.end(m-k-1) = matA.corner(BottomRight,m-k-1,n-k-1) * e.end(n-k-1);
for (j = k+1; j < n; ++j)
matA.col(j).end(m-k-1) += (-e[j]/e[k+1]) * work.end(m-k-1);
}
// Place the transformation in V for subsequent back multiplication.
if (wantv)
m_matV.col(k).end(n-k-1) = e.end(n-k-1);
}
}
// Set up the final bidiagonal matrix or order p.
int p = std::min(n,m+1);
if (nct < n)
m_sigma[nct] = matA(nct,nct);
if (m < p)
m_sigma[p-1] = 0.0;
if (nrt+1 < p)
e[nrt] = matA(nrt,p-1);
e[p-1] = 0.0;
// If required, generate U.
if (wantu)
{
for (j = nct; j < nu; ++j)
{
m_matU.col(j).setZero();
m_matU(j,j) = 1.0;
}
for (k = nct-1; k >= 0; k--)
{
if (m_sigma[k] != 0.0)
{
for (j = k+1; j < nu; ++j)
{
Scalar t = m_matU.col(k).end(m-k).dot(m_matU.col(j).end(m-k)); // FIXME is it really a dot product we want ?
t = -t/m_matU(k,k);
m_matU.col(j).end(m-k) += t * m_matU.col(k).end(m-k);
}
m_matU.col(k).end(m-k) = - m_matU.col(k).end(m-k);
m_matU(k,k) = Scalar(1) + m_matU(k,k);
if (k-1>0)
m_matU.col(k).start(k-1).setZero();
}
else
{
m_matU.col(k).setZero();
m_matU(k,k) = 1.0;
}
}
}
// If required, generate V.
if (wantv)
{
for (k = n-1; k >= 0; k--)
{
if ((k < nrt) & (e[k] != 0.0))
{
for (j = k+1; j < nu; ++j)
{
Scalar t = m_matV.col(k).end(n-k-1).dot(m_matV.col(j).end(n-k-1)); // FIXME is it really a dot product we want ?
t = -t/m_matV(k+1,k);
m_matV.col(j).end(n-k-1) += t * m_matV.col(k).end(n-k-1);
}
}
m_matV.col(k).setZero();
m_matV(k,k) = 1.0;
}
}
// Main iteration loop for the singular values.
int pp = p-1;
int iter = 0;
Scalar eps = ei_pow(Scalar(2),ei_is_same_type<Scalar,float>::ret ? Scalar(-23) : Scalar(-52));
while (p > 0)
{
int k=0;
int kase=0;
// Here is where a test for too many iterations would go.
// This section of the program inspects for
// negligible elements in the s and e arrays. On
// completion the variables kase and k are set as follows.
// kase = 1 if s(p) and e[k-1] are negligible and k<p
// kase = 2 if s(k) is negligible and k<p
// kase = 3 if e[k-1] is negligible, k<p, and
// s(k), ..., s(p) are not negligible (qr step).
// kase = 4 if e(p-1) is negligible (convergence).
for (k = p-2; k >= -1; --k)
{
if (k == -1)
break;
if (ei_abs(e[k]) <= eps*(ei_abs(m_sigma[k]) + ei_abs(m_sigma[k+1])))
{
e[k] = 0.0;
break;
}
}
if (k == p-2)
{
kase = 4;
}
else
{
int ks;
for (ks = p-1; ks >= k; --ks)
{
if (ks == k)
break;
Scalar t = (ks != p ? ei_abs(e[ks]) : Scalar(0)) + (ks != k+1 ? ei_abs(e[ks-1]) : Scalar(0));
if (ei_abs(m_sigma[ks]) <= eps*t)
{
m_sigma[ks] = 0.0;
break;
}
}
if (ks == k)
{
kase = 3;
}
else if (ks == p-1)
{
kase = 1;
}
else
{
kase = 2;
k = ks;
}
}
++k;
// Perform the task indicated by kase.
switch (kase)
{
// Deflate negligible s(p).
case 1:
{
Scalar f(e[p-2]);
e[p-2] = 0.0;
for (j = p-2; j >= k; --j)
{
Scalar t(ei_hypot(m_sigma[j],f));
Scalar cs(m_sigma[j]/t);
Scalar sn(f/t);
m_sigma[j] = t;
if (j != k)
{
f = -sn*e[j-1];
e[j-1] = cs*e[j-1];
}
if (wantv)
{
for (i = 0; i < n; ++i)
{
t = cs*m_matV(i,j) + sn*m_matV(i,p-1);
m_matV(i,p-1) = -sn*m_matV(i,j) + cs*m_matV(i,p-1);
m_matV(i,j) = t;
}
}
}
}
break;
// Split at negligible s(k).
case 2:
{
Scalar f(e[k-1]);
e[k-1] = 0.0;
for (j = k; j < p; ++j)
{
Scalar t(ei_hypot(m_sigma[j],f));
Scalar cs( m_sigma[j]/t);
Scalar sn(f/t);
m_sigma[j] = t;
f = -sn*e[j];
e[j] = cs*e[j];
if (wantu)
{
for (i = 0; i < m; ++i)
{
t = cs*m_matU(i,j) + sn*m_matU(i,k-1);
m_matU(i,k-1) = -sn*m_matU(i,j) + cs*m_matU(i,k-1);
m_matU(i,j) = t;
}
}
}
}
break;
// Perform one qr step.
case 3:
{
// Calculate the shift.
Scalar scale = std::max(std::max(std::max(std::max(
ei_abs(m_sigma[p-1]),ei_abs(m_sigma[p-2])),ei_abs(e[p-2])),
ei_abs(m_sigma[k])),ei_abs(e[k]));
Scalar sp = m_sigma[p-1]/scale;
Scalar spm1 = m_sigma[p-2]/scale;
Scalar epm1 = e[p-2]/scale;
Scalar sk = m_sigma[k]/scale;
Scalar ek = e[k]/scale;
Scalar b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)/Scalar(2);
Scalar c = (sp*epm1)*(sp*epm1);
Scalar shift = 0.0;
if ((b != 0.0) || (c != 0.0))
{
shift = ei_sqrt(b*b + c);
if (b < 0.0)
shift = -shift;
shift = c/(b + shift);
}
Scalar f = (sk + sp)*(sk - sp) + shift;
Scalar g = sk*ek;
// Chase zeros.
for (j = k; j < p-1; ++j)
{
Scalar t = ei_hypot(f,g);
Scalar cs = f/t;
Scalar sn = g/t;
if (j != k)
e[j-1] = t;
f = cs*m_sigma[j] + sn*e[j];
e[j] = cs*e[j] - sn*m_sigma[j];
g = sn*m_sigma[j+1];
m_sigma[j+1] = cs*m_sigma[j+1];
if (wantv)
{
for (i = 0; i < n; ++i)
{
t = cs*m_matV(i,j) + sn*m_matV(i,j+1);
m_matV(i,j+1) = -sn*m_matV(i,j) + cs*m_matV(i,j+1);
m_matV(i,j) = t;
}
}
t = ei_hypot(f,g);
cs = f/t;
sn = g/t;
m_sigma[j] = t;
f = cs*e[j] + sn*m_sigma[j+1];
m_sigma[j+1] = -sn*e[j] + cs*m_sigma[j+1];
g = sn*e[j+1];
e[j+1] = cs*e[j+1];
if (wantu && (j < m-1))
{
for (i = 0; i < m; ++i)
{
t = cs*m_matU(i,j) + sn*m_matU(i,j+1);
m_matU(i,j+1) = -sn*m_matU(i,j) + cs*m_matU(i,j+1);
m_matU(i,j) = t;
}
}
}
e[p-2] = f;
iter = iter + 1;
}
break;
// Convergence.
case 4:
{
// Make the singular values positive.
if (m_sigma[k] <= 0.0)
{
m_sigma[k] = m_sigma[k] < Scalar(0) ? -m_sigma[k] : Scalar(0);
if (wantv)
m_matV.col(k).start(pp+1) = -m_matV.col(k).start(pp+1);
}
// Order the singular values.
while (k < pp)
{
if (m_sigma[k] >= m_sigma[k+1])
break;
Scalar t = m_sigma[k];
m_sigma[k] = m_sigma[k+1];
m_sigma[k+1] = t;
if (wantv && (k < n-1))
m_matV.col(k).swap(m_matV.col(k+1));
if (wantu && (k < m-1))
m_matU.col(k).swap(m_matU.col(k+1));
++k;
}
iter = 0;
p--;
}
break;
} // end big switch
} // end iterations
}
template<typename MatrixType>
SVD<MatrixType>& SVD<MatrixType>::sort()
{
int mu = m_matU.rows();
int mv = m_matV.rows();
int n = m_matU.cols();
for (int i=0; i<n; ++i)
{
int k = i;
Scalar p = m_sigma.coeff(i);
for (int j=i+1; j<n; ++j)
{
if (m_sigma.coeff(j) > p)
{
k = j;
p = m_sigma.coeff(j);
}
}
if (k != i)
{
m_sigma.coeffRef(k) = m_sigma.coeff(i); // i.e.
m_sigma.coeffRef(i) = p; // swaps the i-th and the k-th elements
int j = mu;
for(int s=0; j!=0; ++s, --j)
std::swap(m_matU.coeffRef(s,i), m_matU.coeffRef(s,k));
j = mv;
for (int s=0; j!=0; ++s, --j)
std::swap(m_matV.coeffRef(s,i), m_matV.coeffRef(s,k));
}
}
return *this;
}
/** \returns the solution of \f$ A x = b \f$ using the current SVD decomposition of A.
* The parts of the solution corresponding to zero singular values are ignored.
*
* \sa MatrixBase::svd(), LU::solve(), LLT::solve()
*/
template<typename MatrixType>
template<typename OtherDerived, typename ResultType>
bool SVD<MatrixType>::solve(const MatrixBase<OtherDerived> &b, ResultType* result) const
{
const int rows = m_matU.rows();
ei_assert(b.rows() == rows);
Scalar maxVal = m_sigma.cwise().abs().maxCoeff();
for (int j=0; j<b.cols(); ++j)
{
Matrix<Scalar,MatrixUType::RowsAtCompileTime,1> aux = m_matU.transpose() * b.col(j);
for (int i = 0; i <m_matU.cols(); ++i)
{
Scalar si = m_sigma.coeff(i);
if (ei_isMuchSmallerThan(ei_abs(si),maxVal))
aux.coeffRef(i) = 0;
else
aux.coeffRef(i) /= si;
}
result->col(j) = m_matV * aux;
}
return true;
}
/** Computes the polar decomposition of the matrix, as a product unitary x positive.
*
* If either pointer is zero, the corresponding computation is skipped.
*
* Only for square matrices.
*
* \sa computePositiveUnitary(), computeRotationScaling()
*/
template<typename MatrixType>
template<typename UnitaryType, typename PositiveType>
void SVD<MatrixType>::computeUnitaryPositive(UnitaryType *unitary,
PositiveType *positive) const
{
ei_assert(m_matU.cols() == m_matV.cols() && "Polar decomposition is only for square matrices");
if(unitary) *unitary = m_matU * m_matV.adjoint();
if(positive) *positive = m_matV * m_sigma.asDiagonal() * m_matV.adjoint();
}
/** Computes the polar decomposition of the matrix, as a product positive x unitary.
*
* If either pointer is zero, the corresponding computation is skipped.
*
* Only for square matrices.
*
* \sa computeUnitaryPositive(), computeRotationScaling()
*/
template<typename MatrixType>
template<typename UnitaryType, typename PositiveType>
void SVD<MatrixType>::computePositiveUnitary(UnitaryType *positive,
PositiveType *unitary) const
{
ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices");
if(unitary) *unitary = m_matU * m_matV.adjoint();
if(positive) *positive = m_matU * m_sigma.asDiagonal() * m_matU.adjoint();
}
/** decomposes the matrix as a product rotation x scaling, the scaling being
* not necessarily positive.
*
* If either pointer is zero, the corresponding computation is skipped.
*
* This method requires the Geometry module.
*
* \sa computeScalingRotation(), computeUnitaryPositive()
*/
template<typename MatrixType>
template<typename RotationType, typename ScalingType>
void SVD<MatrixType>::computeRotationScaling(RotationType *rotation, ScalingType *scaling) const
{
ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices");
Scalar x = (m_matU * m_matV.adjoint()).determinant(); // so x has absolute value 1
Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> sv(m_sigma);
sv.coeffRef(0) *= x;
if(scaling) scaling->lazyAssign(m_matV * sv.asDiagonal() * m_matV.adjoint());
if(rotation)
{
MatrixType m(m_matU);
m.col(0) /= x;
rotation->lazyAssign(m * m_matV.adjoint());
}
}
/** decomposes the matrix as a product scaling x rotation, the scaling being
* not necessarily positive.
*
* If either pointer is zero, the corresponding computation is skipped.
*
* This method requires the Geometry module.
*
* \sa computeRotationScaling(), computeUnitaryPositive()
*/
template<typename MatrixType>
template<typename ScalingType, typename RotationType>
void SVD<MatrixType>::computeScalingRotation(ScalingType *scaling, RotationType *rotation) const
{
ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices");
Scalar x = (m_matU * m_matV.adjoint()).determinant(); // so x has absolute value 1
Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> sv(m_sigma);
sv.coeffRef(0) *= x;
if(scaling) scaling->lazyAssign(m_matU * sv.asDiagonal() * m_matU.adjoint());
if(rotation)
{
MatrixType m(m_matU);
m.col(0) /= x;
rotation->lazyAssign(m * m_matV.adjoint());
}
}
/** \svd_module
* \returns the SVD decomposition of \c *this
*/
template<typename Derived>
inline SVD<typename MatrixBase<Derived>::PlainMatrixType>
MatrixBase<Derived>::svd() const
{
return SVD<PlainMatrixType>(derived());
}
#endif // EIGEN_SVD_H

379
ground/openpilotgcs/src/libs/eigen/Eigen/src/Sparse/AmbiVector.h Normal file
View File

@ -0,0 +1,379 @@
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_AMBIVECTOR_H
#define EIGEN_AMBIVECTOR_H
/** \internal
* Hybrid sparse/dense vector class designed for intensive read-write operations.
*
* See BasicSparseLLT and SparseProduct for usage examples.
*/
template<typename _Scalar> class AmbiVector
{
public:
typedef _Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
AmbiVector(int size)
: m_buffer(0), m_size(0), m_allocatedSize(0), m_allocatedElements(0), m_mode(-1)
{
resize(size);
}
void init(RealScalar estimatedDensity);
void init(int mode);
void nonZeros() const;
/** Specifies a sub-vector to work on */
void setBounds(int start, int end) { m_start = start; m_end = end; }
void setZero();
void restart();
Scalar& coeffRef(int i);
Scalar coeff(int i);
class Iterator;
~AmbiVector() { delete[] m_buffer; }
void resize(int size)
{
if (m_allocatedSize < size)
reallocate(size);
m_size = size;
}
int size() const { return m_size; }
protected:
void reallocate(int size)
{
// if the size of the matrix is not too large, let's allocate a bit more than needed such
// that we can handle dense vector even in sparse mode.
delete[] m_buffer;
if (size<1000)
{
int allocSize = (size * sizeof(ListEl))/sizeof(Scalar);
m_allocatedElements = (allocSize*sizeof(Scalar))/sizeof(ListEl);
m_buffer = new Scalar[allocSize];
}
else
{
m_allocatedElements = (size*sizeof(Scalar))/sizeof(ListEl);
m_buffer = new Scalar[size];
}
m_size = size;
m_start = 0;
m_end = m_size;
}
void reallocateSparse()
{
int copyElements = m_allocatedElements;
m_allocatedElements = std::min(int(m_allocatedElements*1.5),m_size);
int allocSize = m_allocatedElements * sizeof(ListEl);
allocSize = allocSize/sizeof(Scalar) + (allocSize%sizeof(Scalar)>0?1:0);
Scalar* newBuffer = new Scalar[allocSize];
std::memcpy(newBuffer, m_buffer, copyElements * sizeof(ListEl));
delete[] m_buffer;
m_buffer = newBuffer;
}
protected:
// element type of the linked list
struct ListEl
{
int next;
int index;
Scalar value;
};
// used to store data in both mode
Scalar* m_buffer;
int m_size;
int m_start;
int m_end;
int m_allocatedSize;
int m_allocatedElements;
int m_mode;
// linked list mode
int m_llStart;
int m_llCurrent;
int m_llSize;
private:
AmbiVector(const AmbiVector&);
};
/** \returns the number of non zeros in the current sub vector */
template<typename Scalar>
void AmbiVector<Scalar>::nonZeros() const
{
if (m_mode==IsSparse)
return m_llSize;
else
return m_end - m_start;
}
template<typename Scalar>
void AmbiVector<Scalar>::init(RealScalar estimatedDensity)
{
if (estimatedDensity>0.1)
init(IsDense);
else
init(IsSparse);
}
template<typename Scalar>
void AmbiVector<Scalar>::init(int mode)
{
m_mode = mode;
if (m_mode==IsSparse)
{
m_llSize = 0;
m_llStart = -1;
}
}
/** Must be called whenever we might perform a write access
* with an index smaller than the previous one.
*
* Don't worry, this function is extremely cheap.
*/
template<typename Scalar>
void AmbiVector<Scalar>::restart()
{
m_llCurrent = m_llStart;
}
/** Set all coefficients of current subvector to zero */
template<typename Scalar>
void AmbiVector<Scalar>::setZero()
{
if (m_mode==IsDense)
{
for (int i=m_start; i<m_end; ++i)
m_buffer[i] = Scalar(0);
}
else
{
ei_assert(m_mode==IsSparse);
m_llSize = 0;
m_llStart = -1;
}
}
template<typename Scalar>
Scalar& AmbiVector<Scalar>::coeffRef(int i)
{
if (m_mode==IsDense)
return m_buffer[i];
else
{
ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_buffer);
// TODO factorize the following code to reduce code generation
ei_assert(m_mode==IsSparse);
if (m_llSize==0)
{
// this is the first element
m_llStart = 0;
m_llCurrent = 0;
++m_llSize;
llElements[0].value = Scalar(0);
llElements[0].index = i;
llElements[0].next = -1;
return llElements[0].value;
}
else if (i<llElements[m_llStart].index)
{
// this is going to be the new first element of the list
ListEl& el = llElements[m_llSize];
el.value = Scalar(0);
el.index = i;
el.next = m_llStart;
m_llStart = m_llSize;
++m_llSize;
m_llCurrent = m_llStart;
return el.value;
}
else
{
int nextel = llElements[m_llCurrent].next;
ei_assert(i>=llElements[m_llCurrent].index && "you must call restart() before inserting an element with lower or equal index");
while (nextel >= 0 && llElements[nextel].index<=i)
{
m_llCurrent = nextel;
nextel = llElements[nextel].next;
}
if (llElements[m_llCurrent].index==i)
{
// the coefficient already exists and we found it !
return llElements[m_llCurrent].value;
}
else
{
if (m_llSize>=m_allocatedElements)
{
reallocateSparse();
llElements = reinterpret_cast<ListEl*>(m_buffer);
}
ei_internal_assert(m_llSize<m_allocatedElements && "internal error: overflow in sparse mode");
// let's insert a new coefficient
ListEl& el = llElements[m_llSize];
el.value = Scalar(0);
el.index = i;
el.next = llElements[m_llCurrent].next;
llElements[m_llCurrent].next = m_llSize;
++m_llSize;
return el.value;
}
}
}
}
template<typename Scalar>
Scalar AmbiVector<Scalar>::coeff(int i)
{
if (m_mode==IsDense)
return m_buffer[i];
else
{
ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_buffer);
ei_assert(m_mode==IsSparse);
if ((m_llSize==0) || (i<llElements[m_llStart].index))
{
return Scalar(0);
}
else
{
int elid = m_llStart;
while (elid >= 0 && llElements[elid].index<i)
elid = llElements[elid].next;
if (llElements[elid].index==i)
return llElements[m_llCurrent].value;
else
return Scalar(0);
}
}
}
/** Iterator over the nonzero coefficients */
template<typename _Scalar>
class AmbiVector<_Scalar>::Iterator
{
public:
typedef _Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
/** Default constructor
* \param vec the vector on which we iterate
* \param epsilon the minimal value used to prune zero coefficients.
* In practice, all coefficients having a magnitude smaller than \a epsilon
* are skipped.
*/
Iterator(const AmbiVector& vec, RealScalar epsilon = RealScalar(0.1)*precision<RealScalar>())
: m_vector(vec)
{
m_epsilon = epsilon;
m_isDense = m_vector.m_mode==IsDense;
if (m_isDense)
{
m_cachedIndex = m_vector.m_start-1;
++(*this);
}
else
{
ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_vector.m_buffer);
m_currentEl = m_vector.m_llStart;
while (m_currentEl>=0 && ei_abs(llElements[m_currentEl].value)<m_epsilon)
m_currentEl = llElements[m_currentEl].next;
if (m_currentEl<0)
{
m_cachedIndex = -1;
}
else
{
m_cachedIndex = llElements[m_currentEl].index;
m_cachedValue = llElements[m_currentEl].value;
}
}
}
int index() const { return m_cachedIndex; }
Scalar value() const { return m_cachedValue; }
operator bool() const { return m_cachedIndex>=0; }
Iterator& operator++()
{
if (m_isDense)
{
do {
++m_cachedIndex;
} while (m_cachedIndex<m_vector.m_end && ei_abs(m_vector.m_buffer[m_cachedIndex])<m_epsilon);
if (m_cachedIndex<m_vector.m_end)
m_cachedValue = m_vector.m_buffer[m_cachedIndex];
else
m_cachedIndex=-1;
}
else
{
ListEl* EIGEN_RESTRICT llElements = reinterpret_cast<ListEl*>(m_vector.m_buffer);
do {
m_currentEl = llElements[m_currentEl].next;
} while (m_currentEl>=0 && ei_abs(llElements[m_currentEl].value)<m_epsilon);
if (m_currentEl<0)
{
m_cachedIndex = -1;
}
else
{
m_cachedIndex = llElements[m_currentEl].index;
m_cachedValue = llElements[m_currentEl].value;
}
}
return *this;
}
protected:
const AmbiVector& m_vector; // the target vector
int m_currentEl; // the current element in sparse/linked-list mode
RealScalar m_epsilon; // epsilon used to prune zero coefficients
int m_cachedIndex; // current coordinate
Scalar m_cachedValue; // current value
bool m_isDense; // mode of the vector
private:
Iterator& operator=(const Iterator&);
};
#endif // EIGEN_AMBIVECTOR_H

6
ground/openpilotgcs/src/libs/eigen/Eigen/src/Sparse/CMakeLists.txt Normal file
View File

@ -0,0 +1,6 @@
FILE(GLOB Eigen_Sparse_SRCS "*.h")
INSTALL(FILES
${Eigen_Sparse_SRCS}
DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Sparse COMPONENT Devel
)

Some files were not shown because too many files have changed in this diff Show More