diff options
Diffstat (limited to 'numlib')
| -rw-r--r-- | numlib/Jamfile | 28 | ||||
| -rw-r--r-- | numlib/LUtest.c | 113 | ||||
| -rw-r--r-- | numlib/License.txt | 662 | ||||
| -rw-r--r-- | numlib/Makefile.am | 15 | ||||
| -rw-r--r-- | numlib/Readme.txt | 14 | ||||
| -rw-r--r-- | numlib/aatree.c | 432 | ||||
| -rw-r--r-- | numlib/aatree.h | 58 | ||||
| -rw-r--r-- | numlib/afiles | 32 | ||||
| -rw-r--r-- | numlib/dhsx.c | 302 | ||||
| -rw-r--r-- | numlib/dhsx.h | 42 | ||||
| -rw-r--r-- | numlib/dnsq.c | 1675 | ||||
| -rw-r--r-- | numlib/dnsq.h | 120 | ||||
| -rw-r--r-- | numlib/dnsqtest.c | 192 | ||||
| -rw-r--r-- | numlib/ludecomp.c | 500 | ||||
| -rw-r--r-- | numlib/ludecomp.h | 93 | ||||
| -rw-r--r-- | numlib/numlib.h | 18 | ||||
| -rw-r--r-- | numlib/numsup.c | 1539 | ||||
| -rw-r--r-- | numlib/numsup.h | 380 | ||||
| -rw-r--r-- | numlib/powell.c | 691 | ||||
| -rw-r--r-- | numlib/powell.h | 74 | ||||
| -rw-r--r-- | numlib/rand.c | 108 | ||||
| -rw-r--r-- | numlib/rand.h | 36 | ||||
| -rw-r--r-- | numlib/sobol.c | 211 | ||||
| -rw-r--r-- | numlib/sobol.h | 52 | ||||
| -rw-r--r-- | numlib/soboltest.c | 127 | ||||
| -rw-r--r-- | numlib/svd.c | 611 | ||||
| -rw-r--r-- | numlib/svd.h | 90 | ||||
| -rw-r--r-- | numlib/svdtest.c | 214 | ||||
| -rw-r--r-- | numlib/tdhsx.c | 117 | ||||
| -rw-r--r-- | numlib/tpowell.c | 123 | ||||
| -rw-r--r-- | numlib/zbrent.c | 182 | ||||
| -rw-r--r-- | numlib/zbrent.h | 44 | ||||
| -rw-r--r-- | numlib/zbrenttest.c | 56 |
33 files changed, 8951 insertions, 0 deletions
diff --git a/numlib/Jamfile b/numlib/Jamfile new file mode 100644 index 0000000..dd90e22 --- /dev/null +++ b/numlib/Jamfile @@ -0,0 +1,28 @@ + +# Jamfile for the numlib and other general utilities + +# Optization state and configuration. Overridden by parent-Jamfiles + +PREF_CCFLAGS = $(CCOPTFLAG) ; # Turn optimisation on +#PREF_CCFLAGS = $(CCDEBUGFLAG) ; # Debugging flags +PREF_LINKFLAGS = $(LINKDEBUGFLAG) ; +#PREF_CCFLAGS = $(CCPROFFLAG) ; # Profile flags +#PREF_LINKFLAGS = $(LINKPROFFLAG) ; # Profile flags + +# Products +Libraries = libnum ; +Headers = numlib.h ; + +#Install +#InstallFile $(DESTDIR)$(PREFIX)/h : $(Headers) ; +#InstallLib $(DESTDIR)$(PREFIX)/lib : $(Libraries) ; + +# Numeric library +Library libnum.lib : numsup.c dnsq.c powell.c dhsx.c ludecomp.c svd.c zbrent.c rand.c sobol.c aatree.c ; + +# Link all utilities with libnum +LINKLIBS = libnum ; + +# All test programs are made from a single source file +MainsFromSources dnsqtest.c tpowell.c tdhsx.c LUtest.c svdtest.c zbrenttest.c soboltest.c ; + diff --git a/numlib/LUtest.c b/numlib/LUtest.c new file mode 100644 index 0000000..feb2277 --- /dev/null +++ b/numlib/LUtest.c @@ -0,0 +1,113 @@ + +/* Test the LU decomposition code */ +/* + * Copyright 1999 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +#include "numlib.h" + +int test(int n, double **a, double *b); + +int main() { + double **a; + double *b; + int n = 4; + int rv; + + a = dmatrix(0, n-1, 0, n-1); + b = dvector(0, n-1); + + a[0][0] = 1.0; + a[0][1] = 2.0; + a[0][2] = 3.1415926; + a[0][3] = 5.1415926; + + a[1][0] = 3.0; + a[1][1] = 2.0; + a[1][2] = 4.0; + a[1][3] = -0.1; + + a[2][0] = -1.0; + a[2][1] = 0.5; + a[2][2] = 1.0; + a[2][3] = 1.5; + + a[3][0] = 11.0; + a[3][1] = 9.0; + a[3][2] = 15.0; + a[3][3] = 0.9; + + b[0] = 6.0; + b[1] = 4.0; + b[2] = 4.5; + b[3] = -10.0; + + if ((rv = test(n, a, b)) != 0) { + if (rv == 1) + printf("LU test failed due to singularity\n"); + else { + printf("LU test failed to verify\n"); + printf("Got solution %f %f %f %f\n",b[0],b[1],b[2],b[3]); + } + } else { + printf("Got verified solution %f %f %f %f\n",b[0],b[1],b[2],b[3]); + } + return 0; +} + + +int +test( +int n, /* Dimensionality */ +double **a, /* A[][] input matrix, returns LU decimposition of A */ +double *b /* B[] input array, returns solution X[] */ +) { + int i, j; + double rip; /* Row interchange parity */ + int *pivx; + int rv = 0; + + double **sa; /* save input matrix values */ + double *sb; /* save input vector values */ + + pivx = ivector(0, n-1); + sa = dmatrix(0, n-1, 0, n-1); + sb = dvector(0, n-1); + + /* Copy input matrix and vector values */ + for (i = 0; i < n; i++) { + sb[i] = b[i]; + for (j = 0; j < n; j++) + sa[i][j] = a[i][j]; + } + + if (lu_decomp(a, n, pivx, &rip)) { + free_dvector(sb, 0, n-1); + free_dmatrix(sa, 0, n-1, 0, n-1); + free_ivector(pivx, 0, n-1); + return 1; + } + + lu_backsub(a, n, pivx, b); + + /* Check that the solution is correct */ + for (i = 0; i < n; i++) { + double sum, temp; + sum = 0.0; + for (j = 0; j < n; j++) + sum += sa[i][j] * b[j]; +//printf("~~ check %d = %f, against %f\n",i,sum,sb[i]); + temp = fabs(sum - sb[i]); + if (temp > 1e-6) + rv = 2; + } + free_dvector(sb, 0, n-1); + free_dmatrix(sa, 0, n-1, 0, n-1); + free_ivector(pivx, 0, n-1); + return rv; +} + diff --git a/numlib/License.txt b/numlib/License.txt new file mode 100644 index 0000000..a871fcf --- /dev/null +++ b/numlib/License.txt @@ -0,0 +1,662 @@ + GNU AFFERO GENERAL PUBLIC LICENSE + Version 3, 19 November 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 Affero General Public License is a free, copyleft license for +software and other kinds of works, specifically designed to ensure +cooperation with the community in the case of network server software. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +our General Public Licenses are 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. + + 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. + + Developers that use our General Public Licenses protect your rights +with two steps: (1) assert copyright on the software, and (2) offer +you this License which gives you legal permission to copy, distribute +and/or modify the software. + + A secondary benefit of defending all users' freedom is that +improvements made in alternate versions of the program, if they +receive widespread use, become available for other developers to +incorporate. Many developers of free software are heartened and +encouraged by the resulting cooperation. However, in the case of +software used on network servers, this result may fail to come about. +The GNU General Public License permits making a modified version and +letting the public access it on a server without ever releasing its +source code to the public. + + The GNU Affero General Public License is designed specifically to +ensure that, in such cases, the modified source code becomes available +to the community. It requires the operator of a network server to +provide the source code of the modified version running there to the +users of that server. Therefore, public use of a modified version, on +a publicly accessible server, gives the public access to the source +code of the modified version. + + An older license, called the Affero General Public License and +published by Affero, was designed to accomplish similar goals. This is +a different license, not a version of the Affero GPL, but Affero has +released a new version of the Affero GPL which permits relicensing under +this license. + + 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 Affero 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. Remote Network Interaction; Use with the GNU General Public License. + + Notwithstanding any other provision of this License, if you modify the +Program, your modified version must prominently offer all users +interacting with it remotely through a computer network (if your version +supports such interaction) an opportunity to receive the Corresponding +Source of your version by providing access to the Corresponding Source +from a network server at no charge, through some standard or customary +means of facilitating copying of software. This Corresponding Source +shall include the Corresponding Source for any work covered by version 3 +of the GNU General Public License that is incorporated pursuant to the +following paragraph. + + 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 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 work with which it is combined will remain governed by version +3 of the GNU General Public License. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU Affero 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 Affero 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 Affero 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 Affero 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 Affero 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 Affero General Public License for more details. + + You should have received a copy of the GNU Affero 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 your software can interact with users remotely through a computer +network, you should also make sure that it provides a way for users to +get its source. For example, if your program is a web application, its +interface could display a "Source" link that leads users to an archive +of the code. There are many ways you could offer source, and different +solutions will be better for different programs; see section 13 for the +specific requirements. + + 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 AGPL, see +<http://www.gnu.org/licenses/>. + diff --git a/numlib/Makefile.am b/numlib/Makefile.am new file mode 100644 index 0000000..da95596 --- /dev/null +++ b/numlib/Makefile.am @@ -0,0 +1,15 @@ +include $(top_srcdir)/Makefile.shared + +privatelib_LTLIBRARIES = libargyllnum.la +privatelibdir = $(pkglibdir) + +libargyllnum_la_SOURCES = numlib.h numsup.c numsup.h dnsq.c dnsq.h \ + powell.c powell.h dhsx.c dhsx.h ludecomp.c ludecomp.h svd.c \ + svd.h zbrent.c zbrent.h rand.c rand.h sobol.c sobol.h aatree.c + +LDADD = ./libargyllnum.la + +check_PROGRAMS = dnsqtest tpowell tdhsx LUtest svdtest zbrenttest \ + soboltest + +EXTRA_DIST = License.txt Readme.txt diff --git a/numlib/Readme.txt b/numlib/Readme.txt new file mode 100644 index 0000000..8cc0173 --- /dev/null +++ b/numlib/Readme.txt @@ -0,0 +1,14 @@ +Collection of numerical routines used by various other +(mainly color) code. + +Included are: + +numsup Error handling, vector/array allocation, macros +dnsq Non-linear equation solver +powell Powell and Conjugate Gradient multi dimentional minimiser +ludecomp LU decomposition matrix solver +svd Singular Value decomposition matrix solver +zbrent 1 dimentional brent root search +rand Random number generators +sobolo Sobol sub-random vector sequence generator + diff --git a/numlib/aatree.c b/numlib/aatree.c new file mode 100644 index 0000000..2f38b1d --- /dev/null +++ b/numlib/aatree.c @@ -0,0 +1,432 @@ + +/* + Andersson binary balanced tree library + + > Created (Julienne Walker): September 10, 2005 + > Corrections (James Bucanek): April 10, 2006 + 1) Typo in jsw_aerase: + up != 0 should be top != 0 + 2) Bug in jsw_aerase: + skew ( path[top] ) should be skew ( up ) + split ( path[top] ) should be split ( up ) + 3) Bug in skew and split macros: + Condition should test for nil + 4) Bug in jsw_aerase: + Search for successor should save the path + > Fixed duplicate handling (Graeme W. Gill): August 16, 2009 +*/ + +#include "aatree.h" + +#ifdef __cplusplus +#include <cstdlib> + +using std::malloc; +using std::free; +using std::size_t; +#else +#include <stdlib.h> +#endif + +#ifndef HEIGHT_LIMIT +#define HEIGHT_LIMIT 64 /* Tallest allowable tree */ +#endif + +typedef struct aat_anode { + int level; /* Horizontal level for balance */ + void *data; /* User-defined content */ + struct aat_anode *link[2]; /* Left (0) and right (1) links */ +} aat_anode_t; + +struct aat_atree { + aat_anode_t *root; /* Top of the tree */ + aat_anode_t *nil; /* End of tree sentinel */ + cmp_f cmp; /* Compare two items */ + size_t size; /* Number of items (user-defined) */ +}; + +struct aat_atrav { + aat_atree_t *tree; /* Paired tree */ + aat_anode_t *it; /* Current node */ + aat_anode_t *path[HEIGHT_LIMIT]; /* Traversal path */ + size_t top; /* Top of stack */ +}; + +/* Remove left horizontal links */ +#define skew(t) do { \ + if ( t->link[0]->level == t->level && t->level != 0 ) { \ + aat_anode_t *save = t->link[0]; \ + t->link[0] = save->link[1]; \ + save->link[1] = t; \ + t = save; \ + } \ +} while(0) + +/* Remove consecutive horizontal links */ +#define split(t) do { \ + if ( t->link[1]->link[1]->level == t->level && t->level != 0 ) { \ + aat_anode_t *save = t->link[1]; \ + t->link[1] = save->link[0]; \ + save->link[0] = t; \ + t = save; \ + ++t->level; \ + } \ +} while(0) + +/* Version of cmp that ensure there are no duplicates */ +/* Set res to comparison result */ +#define AAT_CMP( res, tree, p1, p2) \ +{ \ + if ((res = tree->cmp ( p1, p2 )) == 0) { \ + if (p1 < p2) \ + res = -1; \ + else if (p1 > p2) \ + res = 1; \ + } \ +} + +/* Create a new node that points to our data */ +/* Return tree-nil if malloc failed. */ +static aat_anode_t *new_node ( aat_atree_t *tree, void *data ) +{ + aat_anode_t *rn = (aat_anode_t *)malloc ( sizeof *rn ); + + if ( rn == NULL ) + return tree->nil; + + rn->level = 1; + rn->data = data; + rn->link[0] = rn->link[1] = tree->nil; + + return rn; +} + +/* Create a new empty tree */ +aat_atree_t *aat_anew ( cmp_f cmp ) +{ + aat_atree_t *rt = (aat_atree_t *)malloc ( sizeof *rt ); + + if ( rt == NULL ) + return NULL; + + /* Initialize sentinel */ + rt->nil = (aat_anode_t *)malloc ( sizeof *rt->nil ); + if ( rt->nil == NULL ) { + free ( rt ); + return NULL; + } + + rt->nil->data = NULL; /* Simplifies some ops */ + rt->nil->level = 0; + rt->nil->link[0] = rt->nil->link[1] = rt->nil; + + /* Initialize tree */ + rt->root = rt->nil; + rt->cmp = cmp; + rt->size = 0; + + return rt; +} + +/* Delete the whole tree (but not the data) */ +void aat_adelete ( aat_atree_t *tree ) +{ + aat_anode_t *it = tree->root; + aat_anode_t *save; + + /* Destruction by rotation */ + while ( it != tree->nil ) { + if ( it->link[0] == tree->nil ) { + /* Remove node */ + save = it->link[1]; + free ( it ); + } + else { + /* Rotate right */ + save = it->link[0]; + it->link[0] = save->link[1]; + save->link[1] = it; + } + + it = save; + } + + /* Finalize destruction */ + free ( tree->nil ); + free ( tree ); +} + +/* Locate a entry based on the data's value */ +void *aat_afind ( aat_atree_t *tree, void *data ) +{ + aat_anode_t *it = tree->root; + + while ( it != tree->nil ) { + int cmp; + AAT_CMP(cmp, tree, it->data, data); + + if ( cmp == 0 ) + break; + + it = it->link[cmp < 0]; + } + + /* nil->data == NULL */ + return it->data; +} + +/* Insert an entry based on the data's value. */ +/* Take a copy of the pointer to the entry. */ +/* Return 0 if malloc failed */ +int aat_ainsert ( aat_atree_t *tree, void *data ) +{ + if ( tree->root == tree->nil ) { + /* Empty tree case */ + tree->root = new_node ( tree, data ); + if ( tree->root == tree->nil ) + return 0; + } + else { + aat_anode_t *it = tree->root; + aat_anode_t *path[HEIGHT_LIMIT]; + int top = 0, dir; + + /* Find a spot and save the path */ + for ( ; ; ) { + int cmp; + + path[top++] = it; + AAT_CMP(cmp, tree, it->data, data); + dir = cmp < 0; + + if ( it->link[dir] == tree->nil ) + break; + + it = it->link[dir]; + } + + /* Create a new item */ + it->link[dir] = new_node ( tree, data ); + if ( it->link[dir] == tree->nil ) + return 0; + + /* Walk back and rebalance */ + while ( --top >= 0 ) { + /* Which child? */ + if ( top != 0 ) + dir = path[top - 1]->link[1] == path[top]; + + skew ( path[top] ); + split ( path[top] ); + + /* Fix the parent */ + if ( top != 0 ) + path[top - 1]->link[dir] = path[top]; + else + tree->root = path[top]; + } + } + + ++tree->size; + + return 1; +} + +/* Delete an given entry by locating it by */ +/* its value, then removing it from the tree. */ +/* The item itself isn't deleted. */ +/* Return 0 if the item wasn't found */ +int aat_aerase ( aat_atree_t *tree, void *data ) +{ + if ( tree->root == tree->nil ) + return 0; + else { + aat_anode_t *it = tree->root; + aat_anode_t *path[HEIGHT_LIMIT]; + int top = 0, dir, cmp; + + /* Find node to remove and save path */ + for ( ; ; ) { + path[top++] = it; + + if ( it == tree->nil ) + return 0; + + AAT_CMP(cmp, tree, it->data, data); + if ( cmp == 0 ) + break; + + dir = cmp < 0; + it = it->link[dir]; + } + + /* Remove the found node */ + if ( it->link[0] == tree->nil + || it->link[1] == tree->nil ) + { + /* Single child case */ + int dir2 = it->link[0] == tree->nil; + + /* Unlink the item */ + if ( --top != 0 ) + path[top - 1]->link[dir] = it->link[dir2]; + else + tree->root = it->link[1]; + + free ( it ); + } + else { + /* Two child case */ + aat_anode_t *heir = it->link[1]; + aat_anode_t *prev = it; + + while ( heir->link[0] != tree->nil ) { + path[top++] = prev = heir; + heir = heir->link[0]; + } + + /* + Order is important! + (free item, replace item, free heir) + */ + it->data = heir->data; + prev->link[prev == it] = heir->link[1]; + free ( heir ); + } + + /* Walk back up and rebalance */ + while ( --top >= 0 ) { + aat_anode_t *up = path[top]; + + if ( top != 0 ) + dir = path[top - 1]->link[1] == up; + + /* Rebalance (aka. black magic) */ + if ( up->link[0]->level < up->level - 1 + || up->link[1]->level < up->level - 1 ) + { + if ( up->link[1]->level > --up->level ) + up->link[1]->level = up->level; + + /* Order is important! */ + skew ( up ); + skew ( up->link[1] ); + skew ( up->link[1]->link[1] ); + split ( up ); + split ( up->link[1] ); + } + + /* Fix the parent */ + if ( top != 0 ) + path[top - 1]->link[dir] = up; + else + tree->root = up; + } + } + + --tree->size; + + return 1; +} + +/* Return the current number of items */ +size_t aat_asize ( aat_atree_t *tree ) +{ + return tree->size; +} + +/* Return a traversal object */ +aat_atrav_t *aat_atnew ( void ) +{ + return malloc ( sizeof ( aat_atrav_t ) ); +} + +/* Destroy a traversal object */ +void aat_atdelete ( aat_atrav_t *trav ) +{ + free ( trav ); +} + +/* + First step in traversal, + handles min and max +*/ +static void *start ( aat_atrav_t *trav, + aat_atree_t *tree, int dir ) +{ + trav->tree = tree; + trav->it = tree->root; + trav->top = 0; + + /* Build a path to work with */ + if ( trav->it != tree->nil ) { + while ( trav->it->link[dir] != tree->nil ) { + trav->path[trav->top++] = trav->it; + trav->it = trav->it->link[dir]; + } + } + + /* Could be nil, but nil->data == NULL */ + return trav->it->data; +} + +/* + Subsequent traversal steps, + handles ascending and descending +*/ +static void *move ( aat_atrav_t *trav, int dir ) +{ + aat_anode_t *nil = trav->tree->nil; + + if ( trav->it->link[dir] != nil ) { + /* Continue down this branch */ + trav->path[trav->top++] = trav->it; + trav->it = trav->it->link[dir]; + + while ( trav->it->link[!dir] != nil ) { + trav->path[trav->top++] = trav->it; + trav->it = trav->it->link[!dir]; + } + } + else { + /* Move to the next branch */ + aat_anode_t *last; + + do { + if ( trav->top == 0 ) { + trav->it = nil; + break; + } + + last = trav->it; + trav->it = trav->path[--trav->top]; + } while ( last == trav->it->link[dir] ); + } + + /* Could be nil, but nil->data == NULL */ + return trav->it->data; +} + +/* Return first item */ +void *aat_atfirst ( aat_atrav_t *trav, aat_atree_t *tree ) +{ + return start ( trav, tree, 0 ); /* Min value */ +} + +/* Return last item */ +void *aat_atlast ( aat_atrav_t *trav, aat_atree_t *tree ) +{ + return start ( trav, tree, 1 ); /* Max value */ +} + +/* Move to next */ +void *aat_atnext ( aat_atrav_t *trav ) +{ + return move ( trav, 1 ); /* Toward larger items */ +} + +/* Move to previous */ +void *aat_atprev ( aat_atrav_t *trav ) +{ + return move ( trav, 0 ); /* Toward smaller items */ +} diff --git a/numlib/aatree.h b/numlib/aatree.h new file mode 100644 index 0000000..2a6df84 --- /dev/null +++ b/numlib/aatree.h @@ -0,0 +1,58 @@ +#ifndef AATREE_H +#define AATREE_H + +/* + Andersson binary balanced tree library + + > Created (Julienne Walker): September 10, 2005 + + This code is in the public domain. Anyone may + use it or change it in any way that they see + fit. The author assumes no responsibility for + damages incurred through use of the original + code or any variations thereof. + + It is requested, but not required, that due + credit is given to the original author and + anyone who has modified the code through + a header comment, such as this one. +*/ + +#ifdef __cplusplus +#include <cstddef> + +using std::size_t; + +extern "C" { +#else +#include <stddef.h> +#endif + +/* Opaque types */ +typedef struct aat_atree aat_atree_t; +typedef struct aat_atrav aat_atrav_t; + +/* User-defined item handling */ +typedef int (*cmp_f) ( const void *p1, const void *p2 ); + +/* Andersson tree functions */ +aat_atree_t *aat_anew ( cmp_f cmp ); +void aat_adelete ( aat_atree_t *tree ); +void *aat_afind ( aat_atree_t *tree, void *data ); +int aat_ainsert ( aat_atree_t *tree, void *data ); +int aat_aerase ( aat_atree_t *tree, void *data ); +size_t aat_asize ( aat_atree_t *tree ); + +/* Traversal functions */ +aat_atrav_t *aat_atnew ( void ); +void aat_atdelete ( aat_atrav_t *trav ); +void *aat_atfirst ( aat_atrav_t *trav, aat_atree_t *tree ); +void *aat_atlast ( aat_atrav_t *trav, aat_atree_t *tree ); +void *aat_atnext ( aat_atrav_t *trav ); +void *aat_atprev ( aat_atrav_t *trav ); + +#ifdef __cplusplus +} +#endif + +#endif /* AATREE */ diff --git a/numlib/afiles b/numlib/afiles new file mode 100644 index 0000000..dff62ef --- /dev/null +++ b/numlib/afiles @@ -0,0 +1,32 @@ +Readme.txt +License.txt +afiles +Jamfile +dnsq.c +dnsq.h +dnsqtest.c +numlib.h +numsup.c +numsup.h +powell.h +powell.c +tpowell.c +dhsx.h +dhsx.c +tdhsx.c +rand.c +rand.h +ludecomp.h +ludecomp.c +LUtest.c +svd.h +svd.c +svdtest.c +zbrent.c +zbrent.h +zbrenttest.c +sobol.c +sobol.h +soboltest.c +aatree.h +aatree.c diff --git a/numlib/dhsx.c b/numlib/dhsx.c new file mode 100644 index 0000000..180d15c --- /dev/null +++ b/numlib/dhsx.c @@ -0,0 +1,302 @@ + +/* This is better for stocastic optimisation, where the function */ +/* being evaluated may have a random component, or is not smooth. */ + +/* + * Copyright 1999 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +/* A general purpose downhill simplex multivariate optimser, */ +/* based on the Nelder and Mead algorithm. */ +/* Code is an original expression of the algorithms decsribed in */ +/* "Numerical Recipes in C", by W.H.Press, B.P.Flannery, */ +/* S.A.Teukolsky & W.T.Vetterling. */ + +#include "numsup.h" + +#undef DEBUG + +static void simplexinit(int di, double *cp, double *r,double **p); +static double trypoint(int di,double *cp, double **p, double *y, int hix, double hpfac, + double (*funk)(void *fdata, double *tp), void *fdata, double *tryp); + +#ifdef NEVER /* Experimental */ + +#define ALPHA 0.7 /* Extrapolate hight point through oposite face factor */ +#define GAMMA 1.4 /* Aditional extrapolation if ALPHA is good */ +#define BETA 0.4 /* One dimensional contraction factor */ +#define NONEXP 2 /* non expanding passes */ + +#else /* Standard tuning values */ + +#define ALPHA 1.0 /* Extrapolate hight point through oposite face factor */ +#define GAMMA 2.0 /* Aditional extrapolation if ALPHA is good */ +#define BETA 0.5 /* One dimensional contraction factor */ +#define NONEXP 2 /* non expanding passes */ + +#endif + + +/* Down hill simplex function */ +/* return 0 on sucess, 1 on failure due to excessive itterations */ +/* Result will be in cp */ +/* Arrays start at 0 */ +int dhsx( +double *rv, /* If not NULL, return the residual error */ +int di, /* Dimentionality */ +double *cp, /* Initial starting point */ +double *s, /* Size of initial search area */ +double ftol, /* Finishing tollerance of error change */ +double atol, /* Absolute return value tollerance */ +int maxit, /* Maximum iterations allowed */ +double (*funk)(void *fdata, double *tp), /* Error function to evaluate */ +void *fdata /* Data needed by function */ +) { + int i, j; + int lox, hix, nhix; /* Lowest point index, highest point, next highest point */ + int nsp = di+1; /* Number of simplex verticy points */ + int nit; /* Number of iterations */ + double tryy, ysave; + double tol; + double **p; /* Simplex array */ + double *y; /* values of func at verticies */ + double *tryp; /* Temporary used by trypoint() */ + + /* Allocate array arrays */ + y = dvector(0, di); /* Value of function at verticies */ + tryp = dvector(0, di-1); + p = dmatrix(0, di+1, 0, di); /* Vertex array of dimentions */ + + /* Init the search simplex */ + simplexinit(di, cp, s, p); + + /* Compute current center point position */ + for (j = 0; j < di; j++) { /* For all dimensions */ + double sum; + for (i = 0, sum = 0.0; i < nsp; i++) /* For all verticies */ + sum += p[i][j]; + cp[j] = sum; + } + + /* Compute initial y (function) values at verticies */ + for (i = 0; i < nsp; i++) /* For all verticies */ + y[i] = (*funk)(fdata,p[i]); /* Compute error function */ + + /* Untill we find a solution or give up */ + for (nit = 0; nit < maxit; nit++) { + /* Find highest, next highest and lowest vertex */ + + lox = nhix = hix = 0; + for (i = 0; i < nsp; i++) { + if (y[i] < y[lox]) + lox = i; + if (y[i] > y[hix]) { + nhix = hix; + hix = i; + } else if (y[i] > y[nhix]) { + nhix = i; + } + } + + tol = y[hix] - y[lox]; + +#ifdef DEBUG /* 2D */ + printf("Current vs = %f,%f %f,%f %f,%f\n", + p[0].c[0],p[0].c[1],p[1].c[0],p[1].c[1],p[2].c[0],p[2].c[1]); + printf("Current errs = %e %e %e\n",y[0],y[1],y[2]); + printf("Current sxs = %d %d %d\n",sy[0]->no,sy[1]->no,sy[2]->no); + printf("Current y[hix] = %e\n",y[hix]); +#endif /* DEBUG */ + + if (tol < ftol && y[lox] < atol) { /* Found an adequate solution */ + /* (allow 10 x range for disambiguator) */ + /* set cp[] to best point, and return error value of that point */ + tryy = 1.0/(di+1); + for (j = 0; j < di; j++) /* For all dimensions */ + cp[j] *= tryy; /* Set cp to center point of simplex */ +#ifdef DEBUG + printf("C point = %f,%f\n",cp[0],cp[1]); +#endif + tryy = (*funk)(fdata,cp); /* Compute error function */ + + if (tryy > y[lox]) { /* Center point is not the best */ + tryy = y[lox]; + for (j = 0; j < di; j++) + cp[j] = p[lox][j]; + } + free_dmatrix(p, 0, di+1, 0, di); + free_dvector(tryp, 0, di-1); + free_dvector(y, 0, di); +printf("~1 itterations = %d\n",nit); + if (rv != NULL) + *rv = tryy; + return 0; + } + + if (nit > NONEXP) { /* Only try expanding after a couple of iterations */ + /* Try moving the high point through the oposite face by ALPHA */ +#ifdef DEBUG + printf("dhsx: try moving high point %d through oposite face",hix); +#endif + tryy = trypoint(di, cp, p, y, hix, -ALPHA, funk, fdata, tryp); + } + + if (nit > NONEXP && tryy <= y[lox]) { +#ifdef DEBUG + verbose(4,"dhsx: moving high through oposite face worked"); +#endif + /* Gave good result, so continue on in that direction */ + tryy=trypoint(di,cp,p,y,hix,GAMMA,funk,fdata,tryp); + + + } else if (nit <= NONEXP || tryy >= y[nhix]) { + + /* else if ALPHA move made things worse, do a one dimensional */ + /* contraction by a factor BETA */ +#ifdef DEBUG + verbose(4,"dhsx: else try moving contracting point %d, y[ini] = %f",hix,y[hix]); +#endif + ysave = y[hix]; + tryy = trypoint(di, cp, p, y, hix, BETA, funk, fdata, tryp); + + if (tryy >= ysave) { +#ifdef DEBUG + verbose(4,"dhsx: contracting didn't work, try contracting other points to low"); +#endif + /* That still didn't help us, so move all the */ + /* other points towards the high point */ + for (i = 0; i < nsp; i++) { /* For all verts except low */ + if (i != lox) { + for (j = 0; j < di; j++) { /* For all dimensions */ + cp[j] = 0.5 * (p[i][j] + p[lox][j]); + p[i][j] = cp[j]; + } + /* Compute function value for new point */ + y[i] = (*funk)(fdata,p[i]); /* Compute error function */ + } + } + /* Re-compute current center point value */ + for (j = 0; j < di; j++) { + double sum; + for (i = 0,sum = 0.0;i<nsp;i++) + sum += p[i][j]; + cp[j] = sum; + } + } else { +#ifdef DEBUG + verbose(4,"dhsx: contracting point %d worked, tryy = %e, ysave = %e",hix,tryy,ysave); +#endif + } + } + } + free_dmatrix(p, 0, di+1, 0, di); + free_dvector(tryp, 0, di-1); + free_dvector(y, 0, di); + if (rv != NULL) + *rv = tryy; + return 1; /* Failed */ +} + +/* Try moving the high point through the opposite face */ +/* by a factor of fac, and replaces the high point if */ +/* that proves to be better. */ +static double trypoint( +int di, /* Dimentionality */ +double *cp, /* nsp * center coord/Returned coordinate */ +double **p, /* Starting/Current simplex (modified by dhsx) */ +double *y, /* values of func at verticies */ +int hix, /* Index of high point we are moving */ +double hpfac, /* factor to move high point */ +double (*funk)(void *fdata, double tp[]), /* Error function to evaluate */ +void *fdata, /* Data needed by function */ +double *tryp /* temporary array of size di-1 */ +) { + int j; + double tt, tryy; + + /* Compute trial high point */ + tt = (1.0 - hpfac)/di; + for (j = 0; j < di; j++) + tryp[j] = cp[j] * tt - p[hix][j] * (tt - hpfac); + + /* Evaluate trial point */ + tryy = (*funk)(fdata, tryp); /* Compute error function */ + + /* If new high point pos. is better */ + if (tryy < y[hix]) { +#ifdef DEBUG + printf("Try gave improved %e from sx %d",tryy,lsxp->last_fwd->no); +#endif + y[hix] = tryy; /* Replace func val of hi with trial */ + + for (j = 0; j < di; j++) { + cp[j] += tryp[j] - p[hix][j]; /* Recompute cp */ + p[hix][j] = tryp[j]; /* Replace co-ords of hi with trial */ + } + } else { +#ifdef DEBUG + verbose(4,"Try gave worse %e from sx %d",tryy, + lsxp->last_fwd == NULL ? -999 : lsxp->last_fwd->no); +#endif + } + return tryy; /* Function value of trial point */ +} + + +/* Make up an initial simplex for dhsx routine */ +static void +simplexinit( +int di, /* Dimentionality */ +double *cp, /* Initial solution values */ +double *s, /* initial radius for each dimention */ +double **p /* Simplex to initialize */ +) { + double bb; + double hh = 0.5; /* Constant */ + double rr = sqrt(3.0)/2.0; /* Constant */ + int i,j; + for (i = 0; i <= di; i++) { /* For each vertex */ + /* The bounding points form a equalateral simplex */ + /* whose vertexes are on a spehere about the data */ + /* point center. The coordinate sequence is: */ + /* B = sphere radius */ + /* H = 0.5 */ + /* R = sqrt(3)/2 */ + /* 0 0 0 +B */ + /* 0 0 0 -B */ + + /* 0 0 0 +B */ + /* 0 0 +RB -HB */ + /* 0 0 -RB -HB */ + + /* 0 0 0 +B */ + /* 0 0 +RB -HB */ + /* 0 +RRb -HRB -HB */ + /* 0 -RRb -HRB -HB */ + + /* 0 0 0 +B */ + /* 0 0 +RB -HB */ + /* 0 +RRb -HRB -HB */ + /* +RRRb -HRRb -HRB -HB */ + /* -RRRb -HRRb -HRB -HB */ + + /* etc. */ + bb = 1.0; /* Initial unscaled radius */ + for (j = 0; j < di; j++) { /* For each coordinate in vertex */ + if (j > i) + p[i][j] = cp[j] + s[j] * 0.0; /* If beyond last */ + else if (j == i) /* If last non-zero */ + p[i][j] = cp[j] + s[j] * bb; + else if (i == di && j == (di-1)) /* If last of last */ + p[i][j] = cp[j] + s[j] * -1.0 * bb; + else /* If before last */ + p[i][j] = cp[j] + s[j] * -hh * bb; + bb *= rr; + } + } +} + diff --git a/numlib/dhsx.h b/numlib/dhsx.h new file mode 100644 index 0000000..828b8b2 --- /dev/null +++ b/numlib/dhsx.h @@ -0,0 +1,42 @@ +#ifndef DHSX_H +#define DHSX_H + +/* A general purpose downhill simplex multivariate optimser, */ +/* as described in */ +/* "Numerical Recipes in C", by W.H.Press, B.P.Flannery, */ +/* S.A.Teukolsky & W.T.Vetterling. */ + +#ifdef __cplusplus + extern "C" { +#endif + +/* Down hill simplex function */ +/* return err on sucess, -1.0 on failure */ +/* Result will be in cp */ +/* Arrays start at 0 */ + +/* Standard interface for optimizer function */ +/* return 0 on sucess, 1 on failure due to excessive itterations */ +/* Result will be in cp */ +/* Arrays start at 0 */ +int dhsx( +double *rv, /* If not NULL, return the residual error */ +int di, /* Dimentionality */ +double cp[], /* Initial starting point */ +double s[], /* Size of initial search area */ +double ftol, /* Tollerance of error change to stop on */ +double atol, /* Absolute return value tollerance */ +int maxit, /* Maximum iterations allowed */ +double (*funk)(void *fdata, double tp[]), /* Error function to evaluate */ +void *fdata /* Data needed by function */ +); + +double dhsx_funk( /* Return function value */ + void *fdata, /* Opaque data pointer */ + double tp[]); /* Multivriate input value */ + +#ifdef __cplusplus + } +#endif + +#endif /* DHSX_H */ diff --git a/numlib/dnsq.c b/numlib/dnsq.c new file mode 100644 index 0000000..75f00a8 --- /dev/null +++ b/numlib/dnsq.c @@ -0,0 +1,1675 @@ + +/* Concatenated dnsq code */ + +/* + * This concatenation, translation to C and modifications, + * Copyright 1998 Graeme Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +#include "numsup.h" +#include "dnsq.h" /* Public interface definitions */ + +#undef DEBUG + +typedef long int bool; +#ifndef TRUE +# define FALSE (0) +# define TRUE (!FALSE) +#endif + +#ifndef min +#define min(a,b) ((a) <= (b) ? (a) : (b)) +#endif +#ifndef max +#define max(a,b) ((a) >= (b) ? (a) : (b)) +#endif +#ifndef fabs +#define fabs(x) ((x) >= 0.0 ? (x) : -(x)) +#endif + +/* Forward difference jacobian approximation */ +static int dfdjc1( + void *fdata, + int (*fcn)(void *fdata, int n, double *x, double *fvec, int iflag), + int n, double *x, double * fvec, double *fjac, + int ldfjac, int ml, int mu, + double epsfcn, double *wa1, double *wa2); + +/* QR factorization */ +static int dqrfac(int m, int n, double *a, int lda, + bool pivot, int *ipvt, double *sigma, + double *acnorm, double *wa); + +/* Use QR decomposition to form the orthogonal matrix */ +static int dqform(int m, int n, double *q, int ldq, double *wa); + +/* QR decomposition update */ +static int d1updt(int m, int n, double *s, + double *u, double *v, double *w); + +/* Jacobian rotation, called by QR update */ +static int d1mpyq(int m, int n, double *a, int lda, + double *v, double *w); + +/* Compute norm of a vector */ +static double denorm(int n, double *x); + +/* Line search */ +static int ddoglg(int n, double *r, double *diag, double *qtb, + double delta, double *x, double *wa1, double *wa2); + +/***************************************************************/ + +/* + * A simplified interface to dnsq(). + */ + +int dnsqe( + void *fdata, /* Opaque pointer to pass to fcn() and jac() */ + int (*fcn)(void *fdata, int n, double *x, double *fvec, int iflag), + /* Pointer to function we are solving */ + int (*jac)(void *fdata, int n, double *x, double *fvec, double **fjac), + /* Optional function to compute jacobian, NULL if not used */ + int n, /* Number of functions and variables */ + double x[], /* Initial solution estimate, RETURNs final solution */ + double ss, /* Initial search area */ + double fvec[], /* Array that will be RETURNed with thefunction values at the solution */ + double dtol, /* Desired delta tollerance of the solution */ + double atol, /* Desired absolute tollerance of the solution */ + int maxfev, /* Maximum number of function calls. set to 0 for automatic */ + int nprint /* Turn on debugging printouts from func() every nprint itterations */ +) { + int info = 0; /* Return status */ + + int nfev, njev; + int i,j, index, ml, lr, mu; + double epsfcn = ss * ss; /* Jacobian estimate step */ + double factor = ss; /* Initial step size */ + double maxstep = 0.0; /* Subsequent step size (not working ??) */ + + if (maxfev <= 0) { + maxfev = (n + 1) * 100; + if (jac == NULL) + maxfev <<= 1; + } + ml = n - 1; /* number of subdiagonals within the band of the Jacobian matrix. */ + mu = n - 1; /* number of superdiagonals within the band of the Jacobian matrix. */ + + lr = n * (n + 1) / 2; + index = n * 6 + lr; + + /* Call dnsq. */ + info = dnsq(fdata, fcn, jac, NULL, 0, + n, &x[0], &fvec[0], dtol, atol, + maxfev, ml, mu, epsfcn, NULL, factor, maxstep, nprint, + &nfev, &njev); + + if (info == 5) + info = 4; + if (info == 0) + warning("dnsqe: invalid input parameter."); + return info; +} /* dnsqe */ + + + +/***************************************************************/ +/* + * Library: SLATEC + * Category: F2A + * Type: Double precision (SNSQE-S, DNSQE-D) + * Keywords easy-to-use, nonlinear square system, + * powell hybrid method, zeros + * Author: Hiebert, K. L. (SNLA) + * Translated to C by f2c and Graeme W. Gill + * + * 1. Purpose. + * + * The purpose of DNSQ is to find a zero of a system of N nonlinear + * functions in N variables by a modification of the Powell + * hybrid method. The user must provide a subroutine which + * calculates the functions. The user has the option of either to + * provide a subroutine which calculates the Jacobian or to let the + * code calculate it by a forward-difference approximation. + * This code is the combination of the MINPACK codes (Argonne) + * HYBRD and HYBRDJ. + * + * 2. Subroutine and Type Statements. + * + * SUBROUTINE DNSQ(FCN,JAC,N,X,FVEC,FJAC,LDFJAC,XTOL,MAXFEV, + * ML,MU,EPSFCN,DIAG,MODE,FACTOR,NPRINT,INFO,NFEV, + * NJEV,R,LR,QTF,WA1,WA2,WA3,WA4) + * INTEGER N,MAXFEV,ML,MU,MODE,NPRINT,INFO,NFEV,LDFJAC,NJEV,LR + * DOUBLE PRECISION XTOL,EPSFCN,FACTOR + * DOUBLE PRECISION + * X(N),FVEC(N),DIAG(N),FJAC(LDFJAC,N),R(LR),QTF(N), + * WA1(N),WA2(N),WA3(N),WA4(N) + * EXTERNAL FCN,JAC + * + * 3. Parameters. + * + * Parameters designated as input parameters must be specified on + * entry to DNSQ and are not changed on exit, while parameters + * designated as output parameters need not be specified on entry + * and are set to appropriate values on exit from DNSQ. + * + * fcn() is the name of the user-supplied subroutine which calculates + * the functions. fcn() must be declared in an external statement + * in the user calling program, and should be written as follows. + * + * int fcn( + * void *fdata, + * int n, + * double *x, + * double *fvec, + * int iflag); + * { + * calculate the functions at x and + * return this vector in fvec. + * print out vector if iflag == 0 + * return 0 (or < 0 to abort) + * } + * The value 0 should be returned fcn() unless the + * user wants to terminate execution of DNSQ. In this case + * return a negative integer. + * + * jac() is the name of the user-supplied subroutine which calculates + * the Jacobian. If jac!=NULL, then jac() must be declared in an + * external statement in the user calling program, and should be + * written as follows. + * + * int jac( + * void *fdata, + * int n, + * double *x, + * double *fvec, + * double **fjac, + * { + * Calculate the Jacobian at x and return this + * matrix in fjac. fvec contains the function + * values at x and should not be altered. + * return 0 (or < 0 to abort) + * } + * The value 0 should be returned jac() unless the + * user wants to terminate execution of DNSQ. In this case + * return a negative integer. + * + * If jac == NULL, then the code will approximate the + * Jacobian by forward-differencing. + * + * double **sjac; + * sjac is an optional n by n matrix that can hold an initial + * jacobian matrix that will be used in preference to calling the jac() + * function, or to using forward differencing. If the array is provided, + * it will also contain the last jacobian matrix used before exiting. + * If this array is not used, it should be set to NULL. + * + * int startsjac; + * styatsjac is a flag, that when set to non-zero, will cause the + * sjac[] array to be used as the initial jacobian matrix, in preference + * to calling the jac() function, or to using forward differencing. + * + * n is a positive integer input variable set to the number of + * functions and variables. + * + * x is an array of length n. On input x must contain an initial + * estimate of the solution vector. On output x contains the + * final estimate of the solution vector. + * + * fvec is an output array of length n which contains the functions + * evaluated at the output x. + * + * fjac is an output N by N array which contains the orthogonal + * matrix Q produced by the QR factorization of the final + * approximate Jacobian. + * + * ldfjac is a positive integer input variable not less than n + * which specifies the leading dimension of the array fjac. + * + * dtol is a nonnegative input variable. Termination occurs when + * the relative error between two consecutive iterates is at most + * dtol. Therefore, dtol measures the relative error desired in + * the approximate solution. Section 4 contains more details + * about dtol. + * + * tol is a nonnegative input variable. Termination occurs when + * the value of all the function values falls below this threshold. + * Termination occurs when either the dtol or tol condition is met. + * + * maxfev is a positive integer input variable. Termination occurs + * when the number of calls to fcn is at least maxfev by the end + * of an iteration. + * + * ml is a nonnegative integer input variable which specifies the + * number of subdiagonals within the band of the Jacobian matrix. + * If the Jacobian is not banded or jac!=null, set ml to at + * least n - 1. + * + * mu is a nonnegative integer input variable which specifies the + * number of superdiagonals within the band of the Jacobian + * matrix. If the Jacobian is not banded or jac!=null, set mu to at + * least n - 1. + * + * epsfcn is an input variable used in determining a suitable step + * for the forward-difference approximation. This approximation + * assumes that the relative errors in the functions are of the + * order of epsfcn. If epsfcn is less than the machine + * precision, it is assumed that the relative errors in the + * functions are of the order of the machine precision. If + * jac!=null, then epsfcn can be ignored (treat it as a dummy + * argument). + * + * diag is an array of length n. If MODE = 1 (see below), diag is + * internally set. If mode = 2, diag must contain positive + * entries that serve as implicit (multiplicative) scale factors + * for the variables. + * + * mode is an integer input variable. If mode = 1, the variables + * will be scaled internally. If mode = 2, the scaling is + * specified by the input diag. Other values of mode are + * equivalent to mode = 1. + * + * factor is a positive input variable used in determining the + * initial step bound. This bound is set to the product of + * factor and the Euclidean norm of diag*x if nonzero, or else to + * factor itself. In most cases factor should lie in the + * interval (.1,100.). 100. is a generally recommended value. + * + * nprint is an integer input variable that enables controlled + * printing of iterates if it is positive. In this case, fcn is + * called with iflag = 0 at the beginning of the first iteration + * and every nprint iterations thereafter and immediately prior + * to return, with x and fvec available for printing. Appropriate + * print statements must be added to fcn(see example). If nprint + * is not positive, no special calls of fcn with iflag = 0 are + * made. + * + * info is an integer output variable. If the user has terminated + * execution, info is set to the (negative) value of iflag. See + * description of fcn and jac. Otherwise, INFO is set as follows. + * + * INFO = 0 Improper input parameters. + * + * INFO = 1 Relative error between two consecutive iterates is + * at most XTOL. Normal sucessful return value. + * + * INFO = 2 Number of calls to FCN has reached or exceeded + * MAXFEV. + * + * INFO = 3 XTOL is too small. No further improvement in the + * approximate solution X is possible. + * + * INFO = 4 Iteration is not making good progress, as measured + * by the improvement from the last five Jacobian + * evaluations. + * + * INFO = 5 Iteration is not making good progress, as measured + * by the improvement from the last ten iterations. + * Return value if no zero can be found from this starting + * point. + * + * Sections 4 and 5 contain more details about INFO. + * + * nfev is an integer output variable set to the number of calls to + * fcn. + * + * njev is an integer output variable set to the number of calls to + * jac. (If jac==null, then njev is set to zero.) + * + * 4. Successful completion. + * + * The accuracy of DNSQ is controlled by the convergence parameter + * XTOL. This parameter is used in a test which makes a comparison + * between the approximation X and a solution XSOL. DNSQ + * terminates when the test is satisfied. If the convergence + * parameter is less than the machine precision (as defined by the + * function D1MACH(4)), then DNSQ only attempts to satisfy the test + * defined by the machine precision. Further progress is not + * usually possible. + * + * The test assumes that the functions are reasonably well behaved, + * and, if the Jacobian is supplied by the user, that the functions + * and the Jacobian are coded consistently. If these conditions + * are not satisfied, then DNSQ may incorrectly indicate + * convergence. The coding of the Jacobian can be checked by the + * subroutine DCKDER. If the Jacobian is coded correctly or JAC==NULL, + * then the validity of the answer can be checked, for example, by + * rerunning DNSQ with a tighter tolerance. + * + * Convergence Test. If DENORM(Z) denotes the Euclidean norm of a + * vector Z and D is the diagonal matrix whose entries are + * defined by the array DIAG, then this test attempts to + * guarantee that + * + * DENORM(D*(X-XSOL)) .LE. XTOL*DENORM(D*XSOL). + * + * If this condition is satisfied with XTOL = 10**(-K), then the + * larger components of D*X have K significant decimal digits and + * INFO is set to 1. There is a danger that the smaller + * components of D*X may have large relative errors, but the fast + * rate of convergence of DNSQ usually avoids this possibility. + * + * Unless high precision solutions are required, the recommended + * value for XTOL is the square root of the machine precision. + * + * + * 5. Unsuccessful Completion. + * + * Unsuccessful termination of DNSQ can be due to improper input + * parameters, arithmetic interrupts, an excessive number of + * function evaluations, or lack of good progress. + * + * Improper Input Parameters. INFO is set to 0 if + * N .LE. 0, or LDFJAC .LT. N, or + * XTOL .LT. 0.E0, or MAXFEV .LE. 0, or ML .LT. 0, or MU .LT. 0, + * or FACTOR .LE. 0.E0, or LR .LT. (N*(N+1))/2. + * + * Arithmetic Interrupts. If these interrupts occur in the FCN + * subroutine during an early stage of the computation, they may + * be caused by an unacceptable choice of X by DNSQ. In this + * case, it may be possible to remedy the situation by rerunning + * DNSQ with a smaller value of FACTOR. + * + * Excessive Number of Function Evaluations. A reasonable value + * for MAXFEV is 100*(N+1) for JAC!=NULL and 200*(N+1) for JAC==NULL. + * + * If the number of calls to FCN reaches MAXFEV, then this + * indicates that the routine is converging very slowly as + * measured by the progress of FVEC, and INFO is set to 2. This + * + * situation should be unusual because, as indicated below, lack + * of good progress is usually diagnosed earlier by DNSQ, + * causing termination with info = 4 or INFO = 5. + * + * Lack of Good Progress. DNSQ searches for a zero of the system + * + * by minimizing the sum of the squares of the functions. In so + * doing, it can become trapped in a region where the minimum + * does not correspond to a zero of the system and, in this + * situation, the iteration eventually fails to make good + * progress. In particular, this will happen if the system does + * not have a zero. If the system has a zero, rerunning DNSQ + * from a different starting point may be helpful. + * + * + * 6. Characteristics of The Algorithm. + * + * DNSQ is a modification of the Powell Hybrid method. Two of its + * main characteristics involve the choice of the correction as a + * convex combination of the Newton and scaled gradient directions, + * and the updating of the Jacobian by the rank-1 method of + * Broyden. The choice of the correction guarantees (under + * reasonable conditions) global convergence for starting points + * far from the solution and a fast rate of convergence. The + * Jacobian is calculated at the starting point by either the + * user-supplied subroutine or a forward-difference approximation, + * but it is not recalculated until the rank-1 method fails to + * produce satisfactory progress. + * + * Timing. The time required by DNSQ to solve a given problem + * depends on N, the behavior of the functions, the accuracy + * requested, and the starting point. The number of arithmetic + * operations needed by DNSQ is about 11.5*(N**2) to process + * each evaluation of the functions (call to FCN) and 1.3*(N**3) + * to process each evaluation of the Jacobian (call to JAC, + * if JAC!=NULL). Unless FCN and JAC can be evaluated quickly, + * the timing of DNSQ will be strongly influenced by the time + * spent in FCN and JAC. + * + * Storage. DNSQ requires (3*N**2 + 17*N)/2 single precision + * storage locations, in addition to the storage required by the + * program. There are no internally declared storage arrays. + * + * References: M. J. D. Powell, A hybrid method for nonlinear equa- + * tions. In Numerical Methods for Nonlinear Algebraic + * Equations, P. Rabinowitz, Editor. Gordon and Breach, + * 1988. + * + * Routines called: D1MPYQ, D1UPDT, DDOGLG, DENORM, DFDJC1, DQFORM, DQRFAC + * + * Revision history: (YYMMDD) + * 800301 DATE WRITTEN + * 890531 Changed all specific intrinsics to generic. (WRB) + * 890831 Modified array declarations. (WRB) + * 890831 REVISION DATE from Version 3.2 + * 891214 Prologue converted to Version 4.0 format. (BAB) + * 900315 CALLs to XERROR changed to CALLs to XERMSG. (THJ) + * 920501 Reformatted the REFERENCES section. (WRB) + * 960510 Translated to C and features added. (GWG) + */ + +/* Returns status. 0 = OK. */ +int dnsq( + void *fdata, /* Opaque data pointer, passed to called functions */ + int (*fcn)(void *fdata, int n, double *x, double *fvec, int iflag), + /* Pointer to function we are solving */ + int (*jac)(void *fdata, int n, double *x, double *fvec, double **fjac), + /* Optional function to compute jacobian, NULL if not used */ + double **sjac, /* Optional initial jacobian matrix/last jacobian matrix. NULL if not used.*/ + int startsjac, /* Set to non-zero to use sjac as the initial jacobian */ + int n, /* Number of functions and variables */ + double x[], /* Initial solution estimate, RETURNs final solution */ + double fvec[], /* Array that will be RETURNed with thefunction values at the solution */ + double dtol, /* Desired delta tollerance of the solution */ + double atol, /* Desired tollerance of the root (stops on dtol or tol) */ + int maxfev, /* Set excessive Number of Function Evaluations */ + int ml, /* number of subdiagonals within the band of the Jacobian matrix. */ + int mu, /* number of superdiagonals within the band of the Jacobian matrix. */ + double epsfcn, /* determines suitable step for forward-difference approximation */ + double diag[], /* Optional scaling factors, use NULL for internal scaling */ + double factor, /* Determines the initial step bound */ + double maxstep, /* Determines the maximum subsequent step bound (0.0 for none) */ + /* maxstep is NOT WORKING !!! ??? */ + int nprint, /* Turn on debugging printouts from func() every nprint itterations */ + int *nfev, /* RETURNs the number of calls to fcn() */ + int *njev /* RETURNs the number of calls to jac() */ +) { + int info = 0; /* Return status - invalid argument */ + int smode = 0; /* Scaling mode, 1 = internal */ + + /* Internal working arrays */ + double *fjac = NULL; /* n by n array which contains the orthogonal matrix Q */ + /* produced by the QR factorization of the final approximate Jacobian. */ + int ldfjac = n; /* stride of 2d array */ + double **jjac = NULL; /* NR style pointers to fjac */ + + double *r = NULL; /* Array of length (n*(n+1))/2 which contains the upper */ + /* triangular matrix produced by the QR factorization of the */ + /* final approximate Jacobian, stored rowwise. */ + double *qtf = NULL; /* Array of length n which contains the vector (q transpose)*fvec. */ + + double *wa1 = NULL; /* Four working arrays length n */ + double *wa2 = NULL; + double *wa3 = NULL; + double *wa4 = NULL; + + int iwa[1]; /* Pivot swap array (only one element used) */ + + /* Local variables */ + bool jeval; + int iter; + int i, j, k, l, iflag; + int qrflag; /* Set when a valid Q is in fjac[], and R is in r[] */ + int ncsuc; + int nslow1, nslow2, ncfail; + double temp; + double delta = 0.0; + double ratio, fnorm, pnorm; + double xnorm = 0.0; + double fnorm1; + double actred, prered; + double sum; + + /* Allocate out working arrays */ + if (diag == NULL) { /* Internal scaling */ + smode = 1; + diag = dvector(0,n-1); + } + fjac = dvector(0,(n * n)-1); + jjac = convert_dmatrix(fjac,0,n-1,0,ldfjac-1); + r = dvector(0,((n * (n+1))/2)-1); + qtf = dvector(0,n-1); + wa1 = dvector(0,n-1); + wa2 = dvector(0,n-1); + wa3 = dvector(0,n-1); + wa4 = dvector(0,n-1); + + qrflag = 0; + iflag = 0; + *nfev = 0; + *njev = 0; + + /* Check the input parameters for errors. */ + if (n <= 0 || dtol < 0.0 || maxfev <= 0 + || ml < 0 || mu < 0 || factor <= 0.0 || maxstep < 0.0 + || (sjac == NULL && startsjac != 0)) { + goto func_exit; + } + if (!smode) { /* Check the scaling array we were given */ + for (j = 0; j < n; ++j) { + if (diag[j] <= 0.0) { + goto func_exit; + } + } + } + + /* Evaluate the function at the starting point */ + /* and calculate its norm. */ + *nfev = 1; + if ((iflag = (*fcn)(fdata, n, &x[0], &fvec[0], 1)) < 0) + goto func_exit; + fnorm = denorm(n, &fvec[0]); + + /* Initialize iteration counter and monitors. */ + + iter = 1; + ncsuc = 0; + ncfail = 0; + nslow1 = 0; + nslow2 = 0; + + /* Beginning of the outer loop. */ + for (;;) { + jeval = TRUE; + + /* initialize the jacobian matrix. */ + if (startsjac) { /* User provided array */ + for (j = 0; j < n; j++) { + for (i = 0; i < n; i++) + fjac[j * ldfjac + i] = sjac[j][i]; + } + } else if (jac == NULL) { /* Code approximates the jacobian */ + int ti; + iflag = dfdjc1(fdata, fcn, n, &x[0], &fvec[0], &fjac[0], ldfjac, + ml, mu, epsfcn, &wa1[0], &wa2[0]); + ti = ml + mu + 1; + *nfev += min(ti,n); + } else { /* User supplies jacobian function */ + iflag = (*jac)(fdata, n, &x[0], &fvec[0], jjac); + ++(*njev); + } +#ifdef DEBUG +printf("DNSQ: Jacobian initialized\n"); +#endif + + if (iflag < 0) + goto func_exit; + + /* Compute the qr factorization of the jacobian. */ + dqrfac(n, n, &fjac[0], ldfjac, FALSE, iwa, &wa1[0], &wa2[0], &wa3[0]); + + /* On the first iteration and if internal scaling mode set, scale */ + /* according to the norms of the columns of the initial jacobian. */ + /* (wa2[] will contain norms) */ + /* Do this on subsequent itterations too, if a maxstep is set. */ + if (iter == 1 || maxstep > 0.0) { + if (smode) { + for (j = 0; j < n; ++j) { + diag[j] = wa2[j]; + if (wa2[j] == 0.0) { + diag[j] = 1.0; + } + } + } + + /* On the first iteration, calculate the norm of the scaled */ + /* x[], and initialize the step bound delta. */ + for (j = 0; j < n; ++j) + wa3[j] = diag[j] * x[j]; + + xnorm = denorm(n, &wa3[0]); + if (iter == 1) { + delta = factor * xnorm; + if (delta == 0.0) + delta = factor; +#ifdef DEBUG + printf("Initial Delta = %f\n",delta); +#endif /* DEBUG */ + } else { + delta = maxstep * xnorm; + if (delta == 0.0) + delta = maxstep; +#ifdef DEBUG + printf("Subsequent Delta = %f\n",delta); +#endif /* DEBUG */ + } + } + + /* Form (q transpose)*fvec and store in qtf. */ + for (i = 0; i < n; ++i) + qtf[i] = fvec[i]; + + for (j = 0; j < n; ++j) { + if (fjac[j + j * ldfjac] != 0.0) { + sum = 0.0; + for (i = j; i < n; ++i) + sum += fjac[i + j * ldfjac] * qtf[i]; + temp = -sum / fjac[j + j * ldfjac]; + + for (i = j; i < n; ++i) + qtf[i] += fjac[i + j * ldfjac] * temp; + } + } + + /* Copy the triangular factor of the qr factorization into r. */ + for (j = 0; j < n; ++j) { + l = j; + if (j >= 1) { + for (i = 0; i < j; ++i) { + r[l] = fjac[i + j * ldfjac]; + l += (n-1) - i; + } + } + r[l] = wa1[j]; + } + + /* Accumulate the orthogonal factor Q in fjac. */ + dqform(n, n, &fjac[0], ldfjac, &wa1[0]); + + qrflag = 1; + + /* Rescale if necessary. */ + if (smode) { + for (j = 0; j < n; ++j) { + diag[j] = max(diag[j],wa2[j]); + } + } + + /* Beginning of the inner loop. */ + for (;;) { + /* If requested, call fcn to enable printing of iterates. */ + if (nprint > 0) { + if ((iter - 1) % nprint == 0) + if ((iflag = (*fcn)(fdata, n, &x[0], &fvec[0], 0)) < 0) + goto func_exit; + } + +#ifdef DEBUG + /* If the user supplied an array, and there is a valid Q in */ + /* fjac[], and R is in r[], recover the most recent Jacobian */ + /* matrix by multiplying Q by R */ + if (qrflag && sjac) { + for (i = 0; i < n; ++i) { + for (j = 0; j < n; ++j) { + double temp = 0.0; + l = j; + for (k = 0; k <= j; ++k) { + temp += fjac[k * ldfjac + i] * r[l]; + l += (n-1-k); + } + sjac[j][i] = temp; + } + } + printf("Current Jacobian = \n"); + for (j = 0; j < n; ++j) { + for (i = 0; i < n; ++i) { + printf("%f ",sjac[j][i]); + } + printf("\n"); + } + } +#endif /* DEBUG */ + + /* Determine the direction p. */ + ddoglg(n, &r[0], &diag[0], &qtf[0], delta, &wa1[0], &wa2[0], &wa3[0]); + + /* Store the direction p and x + p. calculate the norm of p. */ + /* (wa1[] is X[] output array from ddoglg()) */ + for (j = 0; j < n; ++j) { + wa1[j] = -wa1[j]; + wa2[j] = x[j] + wa1[j]; + wa3[j] = diag[j] * wa1[j]; + } + pnorm = denorm(n, &wa3[0]); + + /* On the first iteration, adjust the initial step bound. */ + /* Do this on subsequent itterations too, if maxstep is set. */ + if (iter == 1 || maxstep > 0.0) { + delta = min(delta,pnorm); +#ifdef DEBUG + if (iter == 1) + printf("First itter Delta = %f\n",delta); + else + printf("Subsequent itter Delta = %f\n",delta); +#endif /* DEBUG */ + } + + /* Evaluate the function at x + p and calculate its norm. */ + ++(*nfev); + if ((iflag = (*fcn)(fdata, n, &wa2[0], &wa4[0], 1)) < 0) + goto func_exit; + fnorm1 = denorm(n, &wa4[0]); + + /* Compute the scaled actual reduction. */ + actred = -1.0; /* Assume norm is made worse */ + if (fnorm1 < fnorm) { /* There was a reduction in the norm */ + double td; + td = fnorm1 / fnorm; + actred = 1.0 - td * td; + } + + /* Compute the scaled predicted reduction. */ + l = 0; + for (i = 0; i < n; ++i) { + sum = 0.0; + for (j = i; j < n; ++j) { + sum += r[l] * wa1[j]; + ++l; + } + wa3[i] = qtf[i] + sum; + } + + temp = denorm(n, &wa3[0]); + prered = 0.0; + if (temp < fnorm) { + double td; + td = temp / fnorm; + prered = 1.0 - td * td; + } + + /* Compute the ratio of the actual to the predicted reduction. */ + ratio = 0.0; /* Assume no improvement */ + if (prered > 0.0) + ratio = actred / prered; +#ifdef DEBUG +printf("DNSQ: actual/predicted ratio = %f\n",ratio); +#endif /* DEBUG */ + + /* Update the step bound. */ + if (ratio < 0.1) { + ncsuc = 0; + ++ncfail; /* Forces jacobian recalc when ncfail == 2 */ + delta = 0.5 * delta; +#ifdef DEBUG +printf("ratio < 0.1 bad, Delta = %f, ncfail = %d\n",delta,ncfail); +#endif /* DEBUG */ + } else { + ncfail = 0; /* Pospone jacobian recalc */ + ++ncsuc; + if (ratio >= 0.5 || ncsuc > 1) { + delta = max(delta, pnorm / 0.5); +#ifdef DEBUG +printf("ratio > 0.1 good, Delta = %f, ncsuc = %d\n",delta,ncsuc); +#endif /* DEBUG */ + } + if (fabs(ratio - 1.0) <= 0.1) { + delta = 2.0 * pnorm; +#ifdef DEBUG +printf("abs(ratio -1.0) <= 0.1 Delta = %f\n",delta); +#endif /* DEBUG */ + } + } + + /* Test for progressing iteration. */ + if (ratio > 0.0001) { +#ifdef DEBUG +printf("Successful itter\n"); +#endif /* DEBUG */ + /* Successful iteration. update x, fvec, and their norms. */ + for (j = 0; j < n; ++j) { + x[j] = wa2[j]; + wa2[j] = diag[j] * x[j]; + fvec[j] = wa4[j]; + } + xnorm = denorm(n, &wa2[0]); + fnorm = fnorm1; + ++iter; + } + +#ifdef DEBUG +printf("DNSQ: actual = %f\n",actred); +#endif /* DEBUG */ + /* Determine the progress of the iteration. */ + ++nslow1; + if (fabs(actred) >= 0.001) + nslow1 = 0; + if (jeval) + ++nslow2; + if (fabs(actred) >= 0.1) + nslow2 = 0; + + /* Test for convergence. */ + if (delta <= dtol * xnorm || fnorm == 0.0) { +#ifdef DEBUG +printf("DNSQ: delta %f <= dtol * xnorm %f || fnorm == %f\n",delta,dtol * xnorm,fnorm); +#endif /* DEBUG */ + info = 1; + goto func_exit; + } + /* Test for root meeting tolerance (GWG) */ + for (j = 0; j < n; ++j) { + if (fabs(fvec[j]) > atol) + break; + } + if (j >= n) { /* All were below tollerance */ +#ifdef DEBUG +printf("DNSQ: fvecs are all <= atol %f\n",atol); +#endif /* DEBUG */ + info = 1; + goto func_exit; + } + + /* Tests for termination and stringent tolerances. */ + if (*nfev >= maxfev) + info = 2; + if (0.1 * max(0.1 * delta, pnorm) <= M_DIVER * xnorm) + info = 3; + if (nslow2 == 5) + info = 4; + if (nslow1 == 10) + info = 5; + if (info != 0) + goto func_exit; + + /* Criterion for recalculating jacobian */ + if (ncfail == 2) { + break; /* Break out of inner loop */ + } + + /* Calculate the rank one modification to the jacobian */ + /* and update qtf if necessary. */ + for (j = 0; j < n; ++j) { + sum = 0.0; + for (i = 0; i < n; ++i) { + sum += fjac[i + j * ldfjac] * wa4[i]; + } + wa2[j] = (sum - wa3[j]) / pnorm; + wa1[j] = diag[j] * (diag[j] * wa1[j] / pnorm); + if (ratio >= 1e-4) { + qtf[j] = sum; + } + } + + /* Compute the qr factorization of the updated jacobian. */ + d1updt(n, n, &r[0], &wa1[0], &wa2[0], &wa3[0]); + d1mpyq(n, n, &fjac[0], ldfjac, &wa2[0], &wa3[0]); + d1mpyq(1, n, &qtf[0], 1, &wa2[0], &wa3[0]); + + jeval = FALSE; + } /* End of the inner loop. */ + } /* End of the outer loop. */ + + + /* Termination, either normal or user imposed. */ +func_exit: + + /* If the user supplied an array, and there is a valid Q in */ + /* fjac[], and R is in r[], recover the most recent Jacobian */ + /* matrix by multiplying Q by R */ + if (qrflag && sjac) { + for (i = 0; i < n; ++i) { + for (j = 0; j < n; ++j) { + double temp = 0.0; + l = j; + for (k = 0; k <= j; ++k) { + temp += fjac[k * ldfjac + i] * r[l]; + l += (n-1-k); + } + sjac[j][i] = temp; + } + } + } + + /* Free our working arrays */ + if (smode) + free_dvector(diag,0,n-1); + free_dvector(fjac,0,(n * n)-1); + free_convert_dmatrix(jjac,0,n-1,0,ldfjac-1); + free_dvector(r,0,((n * (n+1))/2)-1); + free_dvector(qtf,0,n-1); + free_dvector(wa1,0,n-1); + free_dvector(wa2,0,n-1); + free_dvector(wa3,0,n-1); + free_dvector(wa4,0,n-1); + + + if (iflag < 0) + info = iflag; + if (nprint > 0) + (*fcn)(fdata, n, &x[0], &fvec[0], 0); +#ifdef DEBUG + if (info < 0) + printf("dnsq: Execution terminated because user set iflag negative\n"); + if (info == 0) + printf("dnsq: Invalid input parameter\n"); + if (info == 2) + printf("dnsq: Too many function evaluations\n"); + if (info == 3) + printf("dnsq: dtol too small. no further improvement possible\n"); + if (info > 4) + printf("dnsq: Iteration not making good progress\n"); +#endif /* DEBUG */ + return info; +} /* dnsq */ + +/***************************************************************/ +/***************************************************************/ + +/* + * Given an M by N matrix A, this subroutine computes A*Q where + * Q is the product of 2*(N - 1) transformations + * + * GV(N-1)*...*GV(1)*GW(1)*...*GW(N-1) + * + * and GV(I), GW(I) are Givens rotations in the (I,N) plane which + * eliminate elements in the I-th and N-th planes, respectively. + * Q itself is not given, rather the information to recover the + * GV, GW rotations is supplied. + * + */ + +static int d1mpyq( + int m, /* Number of rows of A */ + int n, /* Number of columns of A */ + double *a, /* M by N array */ + int lda, /* stride for a[][] */ + double *v, /* Input array */ + double *w) /* Input array */ +{ + /* Local variables */ + int i, j; + int nm1 = n - 1; + double temp; + double cos_ = 0.0, sin_ = 0.0; + + /* Apply the first set of givens rotations to a. */ + if (nm1 >= 1) { + for (j = n-2; j >= 0; --j) { + if (fabs(v[j]) > 1.0) + cos_ = 1.0 / v[j]; + if (fabs(v[j]) > 1.0) { /* Computing 2nd power */ + sin_ = sqrt(1.0 - cos_ * cos_); + } + if (fabs(v[j]) <= 1.0) { + sin_ = v[j]; + } + if (fabs(v[j]) <= 1.0) { /* Computing 2nd power */ + cos_ = sqrt(1.0 - sin_ * sin_); + } + for (i = 0; i < m; ++i) { + temp = cos_ * a[i + j * lda] - sin_ * a[i + nm1 * lda]; + a[i + nm1 * lda] = sin_ * a[i + j * lda] + cos_ * a[i + nm1 * lda]; + a[i + j * lda] = temp; + } + } + + /* Apply the second set of givens rotations to a. */ + for (j = 0; j < nm1; ++j) { + if (fabs(w[j]) > 1.0) + cos_ = 1.0 / w[j]; + if (fabs(w[j]) > 1.0) { /* Computing 2nd power */ + sin_ = sqrt(1.0 - cos_ * cos_); + } + if (fabs(w[j]) <= 1.0) + sin_ = w[j]; + if (fabs(w[j]) <= 1.0) { /* Computing 2nd power */ + cos_ = sqrt(1.0 - sin_ * sin_); + } + for (i = 0; i < m; ++i) { + temp = cos_ * a[i + j * lda] + sin_ * a[i + nm1 * lda]; + a[i + nm1 * lda] = -sin_ * a[i + j * lda] + cos_ * a[i + nm1 * lda]; + a[i + j * lda] = temp; + } + } + } + return 0; +} + +/***************************************************************/ +/***************************************************************/ + +/* + * Given an M by N lower trapezoidal matrix S, an M-vector U, + * and an N-vector V, the problem is to determine an + * orthogonal matrix Q such that + * + * t + * (S + U*V )*Q + * + * is again lower trapezoidal. + * + * This subroutine determines Q as the product of 2*(N - 1) + * transformations + * + * GV(N-1)*...*GV(1)*GW(1)*...*GW(N-1) + * + * where GV(I), GW(I) are Givens rotations in the (I,N) plane + * which eliminate elements in the I-th and N-th planes, + * respectively. Q itself is not accumulated, rather the + * information to recover the GV, GW rotations is returned. + * + */ + +static int d1updt( + int m, /* Number of rows of S */ + int n, /* Number of columns of S */ + double *s, /* Array of length (n*(2*m-n+1))/2 containing the lower trapezoid matrix */ + double *u, /* U vector lenghth m */ + double *v, /* V vector length n */ + double *w) /* Output array W length m */ +{ + int i, j, l; + int jj; + int nm1 = n - 1; + int nmj; + double temp; + double giant, cotan; + double tan_; + double cos_, sin_, tau; + + /* Giant is the largest magnitude. */ + giant = M_LARGE; + + /* Initialize the diagonal element pointer. */ + jj = n * ((m << 1) - n + 1) / 2 - (m - n) - 1; + + /* Move the nontrivial part of the last column of s into w. */ + l = jj; + for (i = nm1; i < m; ++i) { + w[i] = s[l]; + ++l; + } + + /* Rotate the vector v into a multiple of the n-th unit vector */ + /* in such a way that a spike is introduced into w. */ + if (nm1 >= 1) { + for (nmj = 1; nmj <= nm1; ++nmj) { + j = n - nmj - 1; + jj -= m - j; + w[j] = 0.0; + if (v[j] == 0.0) + continue; + + /* Determine a givens rotation which eliminates the */ + /* j-th element of v. */ + if (fabs(v[nm1]) < fabs(v[j])) { + cotan = v[nm1] / v[j]; + sin_ = 0.5 / sqrt(0.25 + 0.25 * (cotan * cotan)); + cos_ = sin_ * cotan; + tau = 1.0; + if (fabs(cos_) * giant > 1.0) { + tau = 1.0 / cos_; + } + } else { + tan_ = v[j] / v[nm1]; + cos_ = 0.5 / sqrt(0.25 + 0.25 * (tan_ * tan_)); + sin_ = cos_ * tan_; + tau = sin_; + } + + /* Apply the transformation to v and store the information */ + /* necessary to recover the givens rotation. */ + v[nm1] = sin_ * v[j] + cos_ * v[nm1]; + v[j] = tau; + + /* Apply the transformation to s and extend the spike in w. */ + l = jj; + for (i = j; i < m; ++i) { + temp = cos_ * s[l] - sin_ * w[i]; + w[i] = sin_ * s[l] + cos_ * w[i]; + s[l] = temp; + ++l; + } + } + } + + /* Add the spike from the rank 1 update to w. */ + for (i = 0; i < m; ++i) + w[i] += v[nm1] * u[i]; + + /* Eliminate the spike. */ + if (nm1 >= 1) { + for (j = 0; j < nm1; ++j) { + if (w[j] != 0.0) { + /* Determine a givens rotation which eliminates the */ + /* j-th element of the spike. */ + if (fabs(s[jj]) < fabs(w[j])) { + cotan = s[jj] / w[j]; + sin_ = 0.5 / sqrt(0.25 + 0.25 * (cotan * cotan)); + cos_ = sin_ * cotan; + tau = 1.0; + if (fabs(cos_) * giant > 1.0) { + tau = 1.0 / cos_; + } + } else { + tan_ = w[j] / s[jj]; + cos_ = 0.5 / sqrt(0.25 + 0.25 * (tan_ * tan_)); + sin_ = cos_ * tan_; + tau = sin_; + } + + /* Apply the transformation to s and reduce the spike in w. */ + l = jj; + for (i = j; i < m; ++i) { + temp = cos_ * s[l] + sin_ * w[i]; + w[i] = -sin_ * s[l] + cos_ * w[i]; + s[l] = temp; + ++l; + } + + /* Store the information necessary to recover the givens rotation. */ + w[j] = tau; + } + jj += m - j; + } + } + + /* Move w back into the last column of the output s. */ + l = jj; + for (i = nm1; i < m; ++i) { + s[l] = w[i]; + ++l; + } + return 0; +} + +/***************************************************************/ +/***************************************************************/ + +/* + * Given an M by N matrix A, an N by N nonsingular diagonal + * matrix D, an M-vector B, and a positive number DELTA, the + * problem is to determine the convex combination X of the + * Gauss-Newton and scaled gradient directions that minimizes + * (A*X - B) in the least squares sense, subject to the + * restriction that the Euclidean norm of D*X be at most DELTA. + * + * This subroutine completes the solution of the problem + * if it is provided with the necessary information from the + * QR factorization of A. That is, if A = Q*R, where Q has + * orthogonal columns and R is an upper triangular matrix, + * then DDOGLG expects the full upper triangle of R and + * the first N components of (Q transpose)*B. + * + */ + +static int ddoglg( + int n, /* Order of r[] */ + double r[], /* Array of length (n*(n+!))/2 containing upper triangular matrix */ + double diag[], /* Array of length n containing the diagonal elements of the matrix d[] */ + double qtb[], /* Array of length n containing first n elements of vector (Q transpose)*B */ + double delta, /* Upper bound on the Euclidean norm of D*X */ + double x[], /* output array of length N containing the desired direction */ + double wa1[], /* Working arrays length n */ + double wa2[]) +{ + int i, j, k, l; + int jj; + int jp1; + int nm1 = n-1; + double temp; + double alpha, gnorm, qnorm; + double epsmch; + double sgnorm; + double sum; + + /* Epsmch is the machine precision. */ + epsmch = M_DIVER; + + /* First, calculate the gauss-newton direction. */ + jj = n * (n + 1) / 2; + for (k = 0; k < n; ++k) { + j = nm1 - k; + jp1 = j + 1; + jj -= (k+1); + l = jj + 1; + sum = 0.0; + if (n >= (jp1+1)) { + for (i = jp1; i < n; ++i) { + sum += r[l] * x[i]; + ++l; + } + } + + temp = r[jj]; + if (temp == 0.0) { + l = j; + for (i = 0; i <= j; ++i) { /* Computing MAX */ + double dt; + dt = fabs(r[l]); + temp = max(temp,dt); + l += nm1 - i; + } + temp = epsmch * temp; + if (temp == 0.0) { + temp = epsmch; + } + } + x[j] = (qtb[j] - sum) / temp; + } + + + /* Test whether the gauss-newton direction is acceptable. */ + for (j = 0; j < n; ++j) { + wa1[j] = 0.0; + wa2[j] = diag[j] * x[j]; + } + qnorm = denorm(n, &wa2[0]); + if (qnorm <= delta) { + return 0; /* Done - use this direction */ + } + + /* The gauss-newton direction is not acceptable. */ + /* Next, calculate the scaled gradient direction. */ + l = 0; + for (j = 0; j < n; ++j) { + temp = qtb[j]; + for (i = j; i < n; ++i) { + wa1[i] += r[l] * temp; + ++l; + } + wa1[j] /= diag[j]; + } + + /* Calculate the norm of the scaled gradient and test for */ + /* The special case in which the scaled gradient is zero. */ + gnorm = denorm(n, &wa1[0]); + sgnorm = 0.0; + alpha = delta / qnorm; + if (gnorm != 0.0) { + /* Calculate the point along the scaled gradient */ + /* at which the quadratic is minimized. */ + for (j = 0; j < n; ++j) + wa1[j] = wa1[j] / gnorm / diag[j]; + l = 0; + for (j = 0; j < n; ++j) { + sum = 0.0; + for (i = j; i < n; ++i) { + sum += r[l] * wa1[i]; + ++l; + } + wa2[j] = sum; + } + temp = denorm(n, &wa2[0]); + sgnorm = gnorm / temp / temp; + + /* Test whether the scaled gradient direction is acceptable. */ + alpha = 0.0; + if (sgnorm < delta) { + double d0,d1,d2,d3,d4, + /* The scaled gradient direction is not acceptable. */ + /* Finally, calculate the point along the dogleg */ + /* at which the quadratic is minimized. */ + bnorm = denorm(n, &qtb[0]); + d0 = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / delta); + d1 = sgnorm / delta; + d2 = d0 - delta / qnorm; + d3 = delta / qnorm; + d4 = sgnorm / delta; + d0 = d0 - delta / qnorm * (d1 * d1) + + sqrt(d2 * d2 + (1.0 - d3 * d3) * (1.0 - d4 * d4)); + d1 = sgnorm / delta; + alpha = delta / qnorm * (1.0 - d1 * d1) / d0; + } + } + + /* Form appropriate convex combination of the gauss-newton */ + /* direction and the scaled gradient direction. */ + temp = (1.0 - alpha) * min(sgnorm,delta); + for (j = 0; j < n; ++j) + x[j] = temp * wa1[j] + alpha * x[j]; + + return 0; +} /* ddoglg */ + + +/***************************************************************/ +/***************************************************************/ + +/* + * Given an N-vector X, this function calculates the + * Euclidean norm of X. + * + * The Euclidean norm is computed by accumulating the sum of + * squares in three different sums. The sums of squares for the + * small and large components are scaled so that no overflows + * occur. Non-destructive underflows are permitted. Underflows + * and overflows do not occur in the computation of the unscaled + * sum of squares for the intermediate components. + * The definitions of small, intermediate and large components + * depend on two constants, RDWARF and RGIANT. The main + * restrictions on these constants are that RDWARF**2 not + * underflow and RGIANT**2 not overflow. The constants + * given here are suitable for every known computer. + * + */ + +static double denorm( + int n, /* Size of x[] */ + double x[]) /* Input vector */ +{ + if (n < 8) { /* Make it simple and fast */ + double ss = 0.0; + switch (n) { + case 8: + ss += x[7] * x[7]; + case 7: + ss += x[6] * x[6]; + case 6: + ss += x[5] * x[5]; + case 5: + ss += x[4] * x[4]; + case 4: + ss += x[3] * x[3]; + case 3: + ss += x[2] * x[2]; + case 2: + ss += x[1] * x[1]; + case 1: + ss += x[0] * x[0]; + } + return sqrt(ss); + } else { + /* Initialized data */ + static double rdwarf = 3.834e-20; + static double rgiant = 1.304e19; + + /* Local variables */ + static double xabs, x1max, x3max; + static int i; + static double s1, s2, s3, agiant, floatn; + double ret_val, td; + + s1 = 0.0; /* Large component */ + s2 = 0.0; /* Intermedate component */ + s3 = 0.0; /* Small component */ + x1max = 0.0; + x3max = 0.0; + floatn = (double) (n + 1); + agiant = rgiant / floatn; + for (i = 0; i < n; i++) { + xabs = (td = x[i], fabs(td)); + + /* Sum for intermediate components. */ + if (xabs > rdwarf && xabs < agiant) { + td = xabs; /* Computing 2nd power */ + s2 += td * td; + + /* Sum for small components. */ + } else if (xabs <= rdwarf) { + if (xabs <= x3max) { + if (xabs != 0.0) { /* Computing 2nd power */ + td = xabs / x3max; + s3 += td * td; + } + } else { /* Computing 2nd power */ + td = x3max / xabs; + s3 = 1.0 + s3 * (td * td); + x3max = xabs; + } + + /* Sum for large components. */ + } else { + if (xabs <= x1max) { /* Computing 2nd power */ + td = xabs / x1max; + s1 += td * td; + } else { /* Computing 2nd power */ + td = x1max / xabs; + s1 = 1.0 + s1 * (td * td); + x1max = xabs; + } + } + } + + /* Calculation of norm. */ + if (s1 != 0.0) { /* Large is present */ + ret_val = x1max * sqrt(s1 + s2 / x1max / x1max); + } else { /* Medium and small are present */ + if (s2 == 0.0) { + ret_val = x3max * sqrt(s3); /* Small only */ + } else { + if (s2 >= x3max) { /* Medium larger than small */ + ret_val = sqrt(s2 * (1.0 + x3max / s2 * (x3max * s3))); + } else { /* Small large than medium */ + ret_val = sqrt(x3max * (s2 / x3max + x3max * s3)); + } + } + } + return ret_val; + } +} + +/***************************************************************/ +/***************************************************************/ + +/* + * This subroutine computes a forward-difference approximation + * to the N by N Jacobian matrix associated with a specified + * problem of N functions in N variables. If the Jacobian has + * a banded form, then function evaluations are saved by only + * approximating the nonzero terms. + * + */ + +static int dfdjc1( /* Return < 0 if fcn() aborts */ + void *fdata, /* Opaque data pointer to pass to fcn() */ + int (*fcn)(void *fdata, int n, double *x, double *fvec, int iflag), + /* Pointer to function we are solving */ + int n, /* Number of functions and variables */ + double x[], /* Input array size n */ + double fvec[], /* array of length n which must contain the functions evaluated at x[] */ + double fjac[], /* output n by n array containing approximation to the Jacobian matrix at x[] */ + int ldfjac, /* stride of fjac[] */ + int ml, /* Number of subdiagonals within the band of the Jacobian matrix */ + int mu, /* Number of superdiagonals within the band of the Jacobian matrix */ + double epsfcn, /* Step length for the forward-difference approximation */ + double *wa1, /* Working arrays of length n */ + double *wa2) +{ + /* Local variables */ + int iflag = 0; + double temp; + int msum; + double h; + int i, j, k; + double eps; + int nm1 = n-1; + + /* M_DIVER is the machine precision. */ + eps = sqrt((max(epsfcn,M_DIVER))); + msum = ml + mu + 1; + if (msum >= n) { + /* Computation of dense approximate jacobian. */ + for (j = 0; j < n; ++j) { + temp = x[j]; + h = eps * fabs(temp); + if (h == 0.0) + h = eps; + x[j] = temp + h; + if ((iflag = (*fcn)(fdata,n, &x[0], &wa1[0], 1)) < 0) + break; + x[j] = temp; + for (i = 0; i < n; ++i) + fjac[i + j * ldfjac] = (wa1[i] - fvec[i]) / h; + } + } else { + /* Computation of banded approximate jacobian. */ + for (k = 0; k < msum; ++k) { + for (j = k; msum < 0 ? j >= nm1 : j <= nm1; j += msum) { + wa2[j] = x[j]; + h = eps * fabs(wa2[j]); + if (h == 0.0) + h = eps; + x[j] = wa2[j] + h; + } + if ((iflag = (*fcn)(fdata, n, &x[0], &wa1[0], 1)) < 0) + break; + for (j = k; msum < 0 ? j >= nm1 : j <= nm1; j += msum) { + x[j] = wa2[j]; + h = eps * fabs(wa2[j]); + if (h == 0.0) + h = eps; + for (i = 0; i < n; ++i) { + fjac[i + j * ldfjac] = 0.0; + if (i >= j - mu && i <= j + ml) + fjac[i + j * ldfjac] = (wa1[i] - fvec[i]) / h; + } + } + } + } + return iflag; +} /* dfdjc1_ */ + +/***************************************************************/ +/***************************************************************/ + +/* + * This subroutine proceeds from the computed QR factorization of + * an M by N matrix A to accumulate the M by M orthogonal matrix + * Q from its factored form. + * + */ + +static int dqform( + int m, /* No of rows of A and the order of Q. */ + int n, /* No of columns of A. */ + double *q, /* m by m array */ + int ldq, /* stride of q[][] */ + double *wa) /* Working aray length m */ +{ + int i, j, k, l, minmn; + double sum; + + /* Zero out upper triangle of q in the first min(m,n) columns. */ + minmn = min(m,n); + if (minmn >= 2) { + for (j = 1; j < minmn; ++j) { + for (i = 0; i < j; ++i) + q[i + j * ldq] = 0.0; + } + } + + /* Initialize remaining columns to those of the identity matrix. */ + if (m > n) { + for (j = n; j < m; ++j) { + for (i = 0; i < m; ++i) { + q[i + j * ldq] = 0.0; + } + q[j + j * ldq] = 1.0; + } + } + + /* Accumulate q from its factored form. */ + for (l = 0; l < minmn; ++l) { + k = minmn - l - 1; + for (i = k; i < m; ++i) { + wa[i] = q[i + k * ldq]; + q[i + k * ldq] = 0.0; + } + q[k + k * ldq] = 1.0; + if (wa[k] != 0.0) { + for (j = k; j < m; ++j) { + double temp; + sum = 0.0; + for (i = k; i < m; ++i) + sum += q[i + j * ldq] * wa[i]; + temp = sum / wa[k]; + for (i = k; i < m; ++i) + q[i + j * ldq] -= temp * wa[i]; + } + } + } + return 0; +} /* dqform_ */ + +/***************************************************************/ +/***************************************************************/ + +/* + * This subroutine uses Householder transformations with column + * pivoting (optional) to compute a QR factorization of the + * M by N matrix A. That is, DQRFAC determines an orthogonal + * matrix Q, a permutation matrix P, and an upper trapezoidal + * matrix R with diagonal elements of nonincreasing magnitude, + * such that A*P = Q*R. The Householder transformation for + * column K, K = 1,2,...,MIN(M,N), is of the form + * + * T + * I - (1/U(K))*U*U + * + * where U has zeros in the first K-1 positions. The form of + * this transformation and the method of pivoting first + * appeared in the corresponding LINPACK subroutine. + * + */ + +static int dqrfac( + int m, /* Number of rows of a[] */ + int n, /* Number of columns of a[] */ + double *a, /* m by n array */ + int lda, /* stride of a[][] */ + bool pivot, /* TRUE to enforce column pivoting */ + int *ipvt, /* Pivot output array, size n */ + double *sigma, /* Output diagonal elements of R, length n */ + double *acnorm, /* Output norms of A, length n */ + double *wa) /* Working array size n */ +{ + /* Local variables */ + int kmax; + int i, j, k, minmn; + double ajnorm; + int jp1; + double sum; + + /* Compute the initial column norms and initialize several arrays. */ + for (j = 0; j < n; ++j) { + acnorm[j] = denorm(m, &a[j * lda]); + sigma[j] = acnorm[j]; + wa[j] = sigma[j]; + if (pivot) + ipvt[j] = j; + } + + /* Reduce a to r with householder transformations. */ + minmn = min(m,n); + for (j = 0; j < minmn; ++j) { + if (pivot) { + /* Bring the column of largest norm into the pivot position. */ + kmax = j; + for (k = j; k < n; ++k) { + if (sigma[k] > sigma[kmax]) { + kmax = k; + } + } + if (kmax != j) { + for (i = 0; i < m; ++i) { + double temp; + temp = a[i + j * lda]; + a[i + j * lda] = a[i + kmax * lda]; + a[i + kmax * lda] = temp; + } + sigma[kmax] = sigma[j]; + wa[kmax] = wa[j]; + k = ipvt[j]; + ipvt[j] = ipvt[kmax]; + ipvt[kmax] = k; + } + } + + /* Compute the householder transformation to reduce the */ + /* j-th column of a to a multiple of the j-th unit vector. */ + ajnorm = denorm(m - j, &a[j + j * lda]); + if (ajnorm != 0.0) { + if (a[j + j * lda] < 0.0) + ajnorm = -ajnorm; + for (i = j; i < m; ++i) + a[i + j * lda] /= ajnorm; + a[j + j * lda] += 1.0; + + /* Apply the transformation to the remaining columns */ + /* and update the norms. */ + jp1 = j + 1; + if (n > jp1) { + for (k = jp1; k < n; ++k) { + double temp; + sum = 0.0; + for (i = j; i < m; ++i) + sum += a[i + j * lda] * a[i + k * lda]; + temp = sum / a[j + j * lda]; + for (i = j; i < m; ++i) + a[i + k * lda] -= temp * a[i + j * lda]; + if (pivot && sigma[k] != 0.0) { + temp = a[j + k * lda] / sigma[k]; + temp = 1.0 - temp * temp; + sigma[k] *= sqrt((max(0.0,temp))); + temp = sigma[k] / wa[k]; + if (0.05 * (temp * temp) <= M_DIVER) { + sigma[k] = denorm(m - jp1, &a[jp1 + k * lda]); + wa[k] = sigma[k]; + } + } + } + } + } + sigma[j] = -ajnorm; + } + return 0; +} /* dqrfac_ */ + +/***************************************************************/ +/***************************************************************/ + diff --git a/numlib/dnsq.h b/numlib/dnsq.h new file mode 100644 index 0000000..e06e562 --- /dev/null +++ b/numlib/dnsq.h @@ -0,0 +1,120 @@ +#ifndef DNSQ_H +#define DNSQ_H + +/* dnsq non-linear equation solver public interface definition */ +/* + * This concatenation Copyright 1998 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +#ifdef __cplusplus + extern "C" { +#endif + +/* + + return values from dnsq() and dnsqe(): + + 0 Improper input parameters. + + 1 Relative error between two consecutive iterates is at + most XTOL. Normal sucess return value. + + 2 Number of calls to FCN has reached or exceeded MAXFEV. + + 3 XTOL is too small. No further improvement in the + approximate solution X is possible. + + 4 Iteration is not making good progress, as measured by + the improvement from the last five Jacobian evaluations. + + 5 Iteration is not making good progress, as measured + by the improvement from the last ten iterations. + Return value if no zero can be found from this starting + point. + */ + +/* dnsq function */ +int dnsq( + void *fdata, /* Opaque data pointer, passed to called functions */ + int (*fcn)(void *fdata, int n, double *x, double *fvec, int iflag), + /* Pointer to function we are solving */ + int (*jac)(void *fdata, int n, double *x, double *fvec, double **fjac), + /* Optional function to compute jacobian, NULL if not used */ + double **sjac, /* Optional initial jacobian matrix/last jacobian matrix. NULL if not used.*/ + int startsjac, /* Set to non-zero to use sjac as the initial jacobian */ + int n, /* Number of functions and variables */ + double x[], /* Initial solution estimate, RETURNs final solution */ + double fvec[], /* Array that will be RETURNed with thefunction values at the solution */ + double dtol, /* Desired tollerance of the solution */ + double tol, /* Desired tollerance of root */ + int maxfev, /* Set excessive Number of Function Evaluations */ + int ml, /* number of subdiagonals within the band of the Jacobian matrix. */ + int mu, /* number of superdiagonals within the band of the Jacobian matrix. */ + double epsfcn, /* determines suitable step for forward-difference approximation */ + double diag[], /* Optional scaling factors, use NULL for internal scaling */ + double factor, /* Determines the initial step bound */ + double maxstep, /* Determines the maximum subsequent step bound (0.0 for none) */ + int nprint, /* Turn on debugging printouts from func() every nprint itterations */ + int *nfev, /* RETURNs the number of calls to fcn() */ + int *njev /* RETURNs the number of calls to jac() */ +); + +/* User supplied functions */ + +/* calculate the functions at x[] */ +int dnsq_fcn( /* Return < 0 on abort */ + void *fdata, /* Opaque data pointer */ + int n, /* Dimenstionality */ + double *x, /* Multivariate input values */ + double *fvec, /* Multivariate output values */ + int iflag /* Flag set to 0 to trigger debug output */ +); + +/* Calculate Jacobian at x[] */ +int dnsq_jac( /* Return < 0 on abort */ + void *fdata, /* Opaque data pointer */ + int n, /* Dimensionality */ + double *x, /* Point to caluculate Jacobian at (do not alter) */ + double *fvec, /* Function values at x (do not alter) */ + double **fjac /* Return n by n Jacobian in this array */ +); + +#define M_LARGE 1.79e+308 +#define M_DIVER 2.22e-15 + +/* Simplified dnsq() */ +int dnsqe( + void *fdata, /* Opaque pointer to pass to fcn() and jac() */ + int (*fcn)(void *fdata, int n, double *x, double *fvec, int iflag), + /* Pointer to function we are solving */ + int (*jac)(void *fdata, int n, double *x, double *fvec, double **fjac), + /* Optional function to compute jacobian, NULL if not used */ + int n, /* Number of functions and variables */ + double x[], /* Initial solution estimate, RETURNs final solution */ + double ss, /* Initial search area */ + double fvec[], /* Array that will be RETURNed with thefunction values at the solution */ + double dtol, /* Desired tollerance of the solution */ + double tol, /* Desired tollerance of root */ + int maxfev, /* Maximum number of function calls. set to 0 for automatic */ + int nprint /* Turn on debugging printouts from func() every nprint itterations */ +); + +#ifdef __cplusplus + } +#endif + +#endif /* DNSQ_H */ + + + + + + + + + + diff --git a/numlib/dnsqtest.c b/numlib/dnsqtest.c new file mode 100644 index 0000000..3f02819 --- /dev/null +++ b/numlib/dnsqtest.c @@ -0,0 +1,192 @@ + +/* + * Copyright 1999 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +/* Example use of dnsqe() */ +/* */ +/* The problem is to determine the values of X(1), X(2), ..., X(9), */ +/* which solve the system of tridiagonal equations */ +/* */ +/* (3-2*X(1))*X(1) -2*X(2) = -1 */ +/* -X(I-1) + (3-2*X(I))*X(I) -2*X(I+1) = -1, I=2-8 */ +/* -X(8) + (3-2*X(9))*X(9) = -1 */ +/* */ +/* Final approximate solution: */ +/* */ +/* -0.5706545E+00 */ +/* -0.6816283E+00 */ +/* -0.7017325E+00 */ +/* -0.7042129E+00 */ +/* -0.7013690E+00 */ +/* -0.6918656E+00 */ +/* -0.6657920E+00 */ +/* -0.5960342E+00 */ +/* -0.4164121E+00 */ + +#include "numlib.h" + +/* Compute norm of a vector */ +static double denorm(int n, double *x); + +int fcn(void *fdata, int n, double *x, double *fvec, int iflag); + +double expect[9] = { + -0.5706545E+00, + -0.6816283E+00, + -0.7017325E+00, + -0.7042129E+00, + -0.7013690E+00, + -0.6918656E+00, + -0.6657920E+00, + -0.5960342E+00, + -0.4164121E+00 }; + +int main(void) +{ + int n = 9 /* 9 */; /* Problem vector size */ + double x[9]; /* Function input values */ + double fvec[9]; /* Function output values */ + double ss; /* Search area */ + int info, j; + double fnorm; + int nprint = 0; /* Itteration debugging print = off */ + double tol; + + error_program = "dnsqtest"; /* Set global error reporting string */ + + /* Driver for dnsqe example. */ + /* Not supplying Jacobian, use approximation */ + + /* The following starting values provide a rough solution. */ + for (j = 1; j <= 9; ++j) { + x[j - 1] = -1.f; + } + ss = 0.1; + + nprint = 0; + + /* Set tol to the square root of the machine precision. */ + /* Unless high precision solutions are required, */ + /* this is the recommended setting. */ + + tol = sqrt(M_DIVER); + + info = dnsqe(NULL, fcn, NULL, n, x, ss, fvec, tol, tol, 0, nprint); + fnorm = denorm(n, fvec); + fprintf(stdout,"Final L2 norm of the residuals = %e\n",fnorm); + fprintf(stdout,"Exit return value = %d (1 = sucess)\n",info); + fprintf(stdout,"Final approximate solution:\n"); + for (j = 0; j < n; j++) { + fprintf(stdout,"x[%d] = %f, expect %f\n",j,x[j], expect[j]); + } + + return 0; +} /* main() */ + +/* Function being solved */ +int fcn(void *fdata, int n, double *x, double *fvec, int iflag) +{ + double temp, temp1, temp2; + int k; + + /* Function Body */ + for (k = 0; k < n; ++k) { + temp = (3.0 - 2.0 * x[k]) * x[k]; + temp1 = 0.0; + if (k != 0) { + temp1 = x[k-1]; + } + temp2 = 0.0; + if (k != ((n)-1)) + temp2 = x[k+1]; + fvec[k] = temp - temp1 - 2.0 * temp2 + 1.0; + if (iflag == 0) + printf("x[%d] = %f, fvec[%d] + %f\n",k,x[k],k,fvec[k]); + +#ifdef DEBUG +printf("~~ x[%d] = %f, fvec[%d] + %f\n",k,x[k],k,fvec[k]); +#endif /* DEBUG */ + } + /* Return < 0 to abort */ + return 0; +} + +/* - - - - - - - - - - - - - - - - - - - */ + +static double denorm( + int n, /* Size of x[] */ + double x[]) /* Input vector */ +{ + /* Initialized data */ + static double rdwarf = 3.834e-20; + static double rgiant = 1.304e19; + + /* Local variables */ + static double xabs, x1max, x3max; + static int i; + static double s1, s2, s3, agiant, floatn; + double ret_val, td; + + s1 = 0.0; /* Large component */ + s2 = 0.0; /* Intermedate component */ + s3 = 0.0; /* Small component */ + x1max = 0.0; + x3max = 0.0; + floatn = (double) (n + 1); + agiant = rgiant / floatn; + for (i = 0; i < n; i++) { + xabs = (td = x[i], fabs(td)); + + /* Sum for intermediate components. */ + if (xabs > rdwarf && xabs < agiant) { + td = xabs; /* Computing 2nd power */ + s2 += td * td; + + /* Sum for small components. */ + } else if (xabs <= rdwarf) { + if (xabs <= x3max) { + if (xabs != 0.0) { /* Computing 2nd power */ + td = xabs / x3max; + s3 += td * td; + } + } else { /* Computing 2nd power */ + td = x3max / xabs; + s3 = 1.0 + s3 * (td * td); + x3max = xabs; + } + + /* Sum for large components. */ + } else { + if (xabs <= x1max) { /* Computing 2nd power */ + td = xabs / x1max; + s1 += td * td; + } else { /* Computing 2nd power */ + td = x1max / xabs; + s1 = 1.0 + s1 * (td * td); + x1max = xabs; + } + } + } + + /* Calculation of norm. */ + if (s1 != 0.0) { /* Large is present */ + ret_val = x1max * sqrt(s1 + s2 / x1max / x1max); + } else { /* Medium and small are present */ + if (s2 == 0.0) { + ret_val = x3max * sqrt(s3); /* Small only */ + } else { + if (s2 >= x3max) { /* Medium larger than small */ + ret_val = sqrt(s2 * (1.0 + x3max / s2 * (x3max * s3))); + } else { /* Small large than medium */ + ret_val = sqrt(x3max * (s2 / x3max + x3max * s3)); + } + } + } + return ret_val; +} + diff --git a/numlib/ludecomp.c b/numlib/ludecomp.c new file mode 100644 index 0000000..72e014a --- /dev/null +++ b/numlib/ludecomp.c @@ -0,0 +1,500 @@ +/***************************************************/ +/* Linear Simultaeous equation solver */ +/***************************************************/ + +/* General simultaneous equation solver. */ +/* Code was inspired by the algorithm decsribed in section 2.3 */ +/* of "Numerical Recipes in C", by W.H.Press, B.P.Flannery, */ +/* S.A.Teukolsky & W.T.Vetterling. */ + +/* + * Copyright 2000 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +#include "numsup.h" +#include "ludecomp.h" + +#undef DO_POLISH +#undef DO_CHECK + +/* Solve the simultaneous linear equations A.X = B */ +/* Return 1 if the matrix is singular, 0 if OK */ +int +solve_se( +double **a, /* A[][] input matrix, returns LU decomposition of A */ +double *b, /* B[] input array, returns solution X[] */ +int n /* Dimensionality */ +) { + double rip; /* Row interchange parity */ + int *pivx, PIVX[10]; +#if defined(DO_POLISH) || defined(DO_CHECK) + double **sa; /* save input matrix values */ + double *sb; /* save input vector values */ + int i, j; +#endif + + if (n <= 10) + pivx = PIVX; + else + pivx = ivector(0, n-1); + +#if defined(DO_POLISH) || defined(DO_CHECK) + sa = dmatrix(0, n-1, 0, n-1); + sb = dvector(0, n-1); + + /* Copy input matrix and vector values */ + for (i = 0; i < n; i++) { + sb[i] = b[i]; + for (j = 0; j < n; j++) + sa[i][j] = a[i][j]; + } +#endif + + if (lu_decomp(a, n, pivx, &rip)) { +#if defined(DO_POLISH) || defined(DO_CHECK) + free_dvector(sb, 0, n-1); + free_dmatrix(sa, 0, n-1, 0, n-1); +#endif + if (pivx != PIVX) + free_ivector(pivx, 0, n-1); + return 1; + } + + lu_backsub(a, n, pivx, b); + +#ifdef DO_POLISH + lu_polish(sa, a, n, sb, b, pivx); /* Improve the solution */ +#endif + +#ifdef DO_CHECK + /* Check that the solution is correct */ + for (i = 0; i < n; i++) { + double sum, temp; + sum = 0.0; + for (j = 0; j < n; j++) + sum += sa[i][j] * b[j]; + temp = fabs(sum - sb[i]); + if (temp > 1e-6) { + free_dvector(sb, 0, n-1); + free_dmatrix(sa, 0, n-1, 0, n-1); + if (pivx != PIVX) + free_ivector(pivx, 0, n-1); + return 2; + } + } +#endif +#if defined(DO_POLISH) || defined(DO_CHECK) + free_dvector(sb, 0, n-1); + free_dmatrix(sa, 0, n-1, 0, n-1); +#endif + if (pivx != PIVX) + free_ivector(pivx, 0, n-1); + return 0; +} + +/* Solve the simultaneous linear equations A.X = B, with polishing */ +/* Return 1 if the matrix is singular, 0 if OK */ +int +polished_solve_se( +double **a, /* A[][] input matrix, returns LU decomposition of A */ +double *b, /* B[] input array, returns solution X[] */ +int n /* Dimensionality */ +) { + double rip; /* Row interchange parity */ + int *pivx, PIVX[10]; + double **sa; /* save input matrix values */ + double *sb; /* save input vector values */ + int i, j; + + if (n <= 10) + pivx = PIVX; + else + pivx = ivector(0, n-1); + + sa = dmatrix(0, n-1, 0, n-1); + sb = dvector(0, n-1); + + /* Copy source input matrix and vector values */ + for (i = 0; i < n; i++) { + sb[i] = b[i]; + for (j = 0; j < n; j++) + sa[i][j] = a[i][j]; + } + + if (lu_decomp(a, n, pivx, &rip)) { + free_dvector(sb, 0, n-1); + free_dmatrix(sa, 0, n-1, 0, n-1); + if (pivx != PIVX) + free_ivector(pivx, 0, n-1); + return 1; + } + + lu_backsub(a, n, pivx, b); + + lu_polish(sa, a, n, sb, b, pivx); /* Improve the solution */ + +#ifdef DO_CHECK + /* Check that the solution is correct */ + for (i = 0; i < n; i++) { + double sum, temp; + sum = 0.0; + for (j = 0; j < n; j++) + sum += sa[i][j] * b[j]; + temp = fabs(sum - sb[i]); + if (temp > 1e-6) { + free_dvector(sb, 0, n-1); + free_dmatrix(sa, 0, n-1, 0, n-1); + if (pivx != PIVX) + free_ivector(pivx, 0, n-1); + return 2; + } + } +#endif + free_dvector(sb, 0, n-1); + free_dmatrix(sa, 0, n-1, 0, n-1); + if (pivx != PIVX) + free_ivector(pivx, 0, n-1); + return 0; +} + +/* Decompose the square matrix A[][] into lower and upper triangles */ +/* Return 1 if the matrix is singular. */ +int +lu_decomp( +double **a, /* A input array, output upper and lower triangles. */ +int n, /* Dimensionality */ +int *pivx, /* Return pivoting row permutations record */ +double *rip /* Row interchange parity, +/- 1.0, used for determinant */ +) { + int i, j; + double *rscale, RSCALE[10]; /* Implicit scaling of each row */ + + if (n <= 10) + rscale = RSCALE; + else + rscale = dvector(0, n-1); + + /* For each row */ + for (i = 0; i < n; i++) { + double big; + /* For each column in row */ + for (big = 0.0, j=0; j < n; j++) { + double temp; + temp = fabs(a[i][j]); + if (temp > big) + big = temp; + } + if (fabs(big) <= DBL_MIN) { + if (rscale != RSCALE) + free_dvector(rscale, 0, n-1); + return 1; /* singular matrix */ + } + rscale[i] = 1.0/big; /* Save the scaling */ + } + + /* For each column (Crout's method) */ + for (*rip = 1.0, j = 0; j < n; j++) { + double big; + int k, bigi = 0; + + /* For each row */ + for (i = 0; i < j; i++) { + double sum; + sum = a[i][j]; + for (k = 0; k < i; k++) + sum -= a[i][k] * a[k][j]; + a[i][j] = sum; + } + + /* Find largest pivot element */ + for (big = 0.0, i = j; i < n; i++) { + double sum, temp; + + sum = a[i][j]; + for (k = 0; k < j; k++) + sum -= a[i][k] * a[k][j]; + a[i][j] = sum; + + temp = rscale[i] * fabs(sum); /* Figure of merit */ + if (temp >= big) { + big = temp; /* Best so far */ + bigi = i; /* Remember index */ + } + } + + /* If we need to interchange rows */ + if (j != bigi) { + { /* Take advantage of matrix storage to swap pointers to rows */ + double *temp; + temp = a[bigi]; + a[bigi] = a[j]; + a[j] = temp; + } + *rip = -(*rip); /* Another row interchange */ + rscale[bigi] = rscale[j]; /* Interchange scale factor */ + } + + pivx[j] = bigi; /* Record pivot */ + if (fabs(a[j][j]) <= DBL_MIN) { + if (rscale != RSCALE) + free_dvector(rscale, 0, n-1); + return 1; /* Pivot element is zero, so matrix is singular */ + } + + /* Divide by the pivot element */ + if (j != (n-1)) { + double temp; + temp = 1.0/a[j][j]; + for (i = j+1; i < n; i++) + a[i][j] *= temp; + } + } + if (rscale != RSCALE) + free_dvector(rscale, 0, n-1); + return 0; +} + +/* Solve a set of simultaneous equations from the */ +/* LU decomposition, by back substitution. */ +void +lu_backsub( +double **a, /* A[][] LU decomposed matrix */ +int n, /* Dimensionality */ +int *pivx, /* Pivoting row permutations record */ +double *b /* Input B[] vecor, return X[] */ +) { + int i, j; + int nvi; /* When >= 0, indicates non-vanishing B[] index */ + + /* Forward substitution, undo pivoting on the fly */ + for (nvi = -1, i = 0; i < n; i++) { + int px; + double sum; + + px = pivx[i]; + sum = b[px]; + b[px] = b[i]; + if (nvi >= 0) { + for (j = nvi; j < i; j++) + sum -= a[i][j] * b[j]; + } else { + if (sum != 0.0) + nvi = i; /* Found non-vanishing element */ + } + b[i] = sum; + } + + /* Back substitution */ + for (i = (n-1); i >= 0; i--) { + double sum; + sum = b[i]; + for (j = i+1; j < n; j++) + sum -= a[i][j] * b[j]; + b[i] = sum/a[i][i]; + } +} + + +/* Improve a solution of equations */ +void +lu_polish( +double **a, /* Original A[][] matrix */ +double **lua, /* LU decomposition of A[][] */ +int n, /* Dimensionality */ +double *b, /* B[] vector of equation */ +double *x, /* X[] solution to be polished */ +int *pivx /* Pivoting row permutations record */ +) { + int i, j; + double *r, R[10]; /* Residuals */ + + if (n <= 10) + r = R; + else + r = dvector(0, n-1); + + /* Accumulate the residual error */ + for (i = 0; i < n; i++) { + double sum; + sum = -b[i]; + for (j = 0; j < n; j++) + sum += a[i][j] * x[j]; + r[i] = sum; + } + + /* Solve for the error */ + lu_backsub(lua, n, pivx, r); + + /* Subtract error from the old solution */ + for (i = 0; i < n; i++) + x[i] -= r[i]; + + if (r != R) + free_dvector(r, 0, n-1); +} + + +/* Invert a matrix A using lu decomposition */ +/* Return 1 if the matrix is singular, 0 if OK */ +int +lu_invert( +double **a, /* A[][] input matrix, returns inversion of A */ +int n /* Dimensionality */ +) { + int i, j; + double rip; /* Row interchange parity */ + int *pivx, PIVX[10]; + double **y; + + if (n <= 10) + pivx = PIVX; + else + pivx = ivector(0, n-1); + + if (lu_decomp(a, n, pivx, &rip)) { + if (pivx != PIVX) + free_ivector(pivx, 0, n-1); + return 1; + } + + /* Copy lu decomp. to y[][] */ + y = dmatrix(0, n-1, 0, n-1); + for (i = 0; i < n; i++) { + for (j = 0; j < n; j++) { + y[i][j] = a[i][j]; + } + } + + /* Find inverse by columns */ + for (i = 0; i < n; i++) { + for (j = 0; j < n; j++) + a[i][j] = 0.0; + a[i][i] = 1.0; + lu_backsub(y, n, pivx, a[i]); + } + + /* Clean up */ + free_dmatrix(y, 0, n-1, 0, n-1); + if (pivx != PIVX) + free_ivector(pivx, 0, n-1); + + return 0; +} + +#ifdef NEVER /* It's not clear that this is correct */ +int +lu_polished_invert( +double **a, /* A[][] input matrix, returns inversion of A */ +int n /* Dimensionality */ +) { + int i, j, k; + double **aa; /* saved a */ + double **t1, **t2; + + aa = dmatrix(0, n-1, 0, n-1); + t1 = dmatrix(0, n-1, 0, n-1); + t2 = dmatrix(0, n-1, 0, n-1); + + /* Copy a to aa */ + for (i = 0; i < n; i++) { + for (j = 0; j < n; j++) + aa[i][j] = a[i][j]; + } + + /* Invert a */ + if ((i = lu_invert(a, n)) != 0) { + free_dmatrix(aa, 0, n-1, 0, n-1); + free_dmatrix(t1, 0, n-1, 0, n-1); + free_dmatrix(t2, 0, n-1, 0, n-1); + return i; + } + + for (k = 0; k < 10; k++) { + matrix_mult(t1, n, n, aa, n, n, a, n, n); + for (i = 0; i < n; i++) { + for (j = 0; j < n; j++) { + t2[i][j] = a[i][j]; + if (i == j) + t1[i][j] = 2.0 - t1[i][j]; + else + t1[i][j] = 0.0 - t1[i][j]; + } + } + matrix_mult(a, n, n, t2, n, n, t1, n, n); + } + + free_dmatrix(aa, 0, n-1, 0, n-1); + free_dmatrix(t1, 0, n-1, 0, n-1); + free_dmatrix(t2, 0, n-1, 0, n-1); + return 0; +} +#endif + +/* Pseudo-Invert matrix A using lu decomposition */ +/* Return 1 if the matrix is singular, 0 if OK */ +int +lu_psinvert( +double **out, /* Output[0..N-1][0..M-1] */ +double **in, /* Input[0..M-1][0..N-1] input matrix */ +int m, /* Rows */ +int n /* Columns */ +) { + int rv = 0; + double **tr; /* Transpose */ + double **sq; /* Square matrix */ + + tr = dmatrix(0, n-1, 0, m-1); + matrix_trans(tr, in, m, n); + + /* Use left inverse */ + if (m > n) { + sq = dmatrix(0, n-1, 0, n-1); + + /* Multiply transpose by input */ + if ((rv = matrix_mult(sq, n, n, tr, n, m, in, m, n)) == 0) { + + /* Invert the square matrix */ + if ((rv = lu_invert(sq, n)) == 0) { + + /* Multiply inverted square by transpose */ + rv = matrix_mult(out, n, m, sq, n, n, tr, n, m); + } + } + free_dmatrix(sq, 0, n-1, 0, n-1); + + /* Use right inverse */ + } else { + sq = dmatrix(0, m-1, 0, m-1); + + /* Multiply input by transpose */ + if ((rv = matrix_mult(sq, m, m, in, m, n, tr, n, m)) == 0) { + + /* Invert the square matrix */ + if ((rv = lu_invert(sq, m)) == 0) { + + /* Multiply transpose by inverted square */ + rv = matrix_mult(out, n, m, tr, n, m, sq, m, m); + } + } + free_dmatrix(sq, 0, m-1, 0, m-1); + } + + free_dmatrix(tr, 0, n-1, 0, m-1); + + return rv; +} + + + + + + + + + + + diff --git a/numlib/ludecomp.h b/numlib/ludecomp.h new file mode 100644 index 0000000..cdb4382 --- /dev/null +++ b/numlib/ludecomp.h @@ -0,0 +1,93 @@ + +#ifndef LUDECOMP_H +#define LUDECOMP_H + +/* + * Copyright 2000 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +#ifdef __cplusplus + extern "C" { +#endif + +/* NOTE:- lu decomp rearanges the rows of the matrix */ +/* by swapping pointers rather than exchanging data, */ +/* so the matrix must be addressed by the **pointer */ +/* if it is re-used after an ludecomp!!! */ + +/* Solve the simultaneous linear equations A.X = B */ +/* Return 1 if the matrix is singular, 0 if OK */ +int +solve_se( +double **a, /* A[][] input matrix, returns LU decimposition of A */ +double *b, /* B[] input array, returns solution X[] */ +int n /* Dimensionality */ +); + +/* Solve the simultaneous linear equations A.X = B, with polishing */ +/* Return 1 if the matrix is singular, 0 if OK */ +int +polished_solve_se( +double **a, /* A[][] input matrix, returns LU decimposition of A */ +double *b, /* B[] input array, returns solution X[] */ +int n /* Dimensionality */ +); + +/* Decompose the square matrix A[][] into lower and upper triangles */ +/* Return 1 if the matrix is singular. */ +int +lu_decomp( +double **a, /* A input array, output upper and lower triangles. */ +int n, /* Dimensionality */ +int *pivx, /* Return pivoting row permutations record */ +double *rip /* Row interchange parity, +/- 1.0, used for determinant */ +); + +/* Solve a set of simultaneous equations from the */ +/* LU decomposition, by back substitution. */ +void +lu_backsub( +double **a, /* A[][] LU decomposed matrix */ +int n, /* Dimensionality */ +int *pivx, /* Pivoting row permutations record */ +double *b /* Input B[] vecor, return X[] */ +); + +/* Polish a solution for equations */ +void +lu_polish( +double **a, /* Original A[][] matrix */ +double **lua, /* LU decomposition of A[][] */ +int n, /* Dimensionality */ +double *b, /* B[] vector of equation */ +double *x, /* X[] solution to be polished */ +int *pivx /* Pivoting row permutations record */ +); + +/* Invert a matrix A using lu decomposition */ +/* Return 1 if the matrix is singular, 0 if OK */ +int +lu_invert( +double **a, /* A[][] input matrix, returns inversion of A */ +int n /* Dimensionality */ +); + +/* Pseudo-Invert matrix A using lu decomposition */ +/* Return 1 if the matrix is singular, 0 if OK */ +int +lu_psinvert( +double **out, /* Output[0..N-1][0..M-1] */ +double **in, /* Input[0..M-1][0..N-1] input matrix */ +int m, /* Rows */ +int n /* Columns */ +); + +#ifdef __cplusplus + } +#endif + +#endif /* LUDECOMP_H */ diff --git a/numlib/numlib.h b/numlib/numlib.h new file mode 100644 index 0000000..aa34b2d --- /dev/null +++ b/numlib/numlib.h @@ -0,0 +1,18 @@ + +#ifndef NUMLIB_H +#define NUMLIB_H + +/* Numerical routine library interface declarations */ + +#include "numsup.h" /* Support routines, macros */ +#include "dnsq.h" /* Non-linear equation solver */ +#include "powell.h" /* Powell multi dimentional minimiser */ +#include "dhsx.h" /* Downhill simplex multi dimentional minimiser */ +#include "ludecomp.h" /* LU decomposition matrix solver */ +#include "svd.h" /* Singular Value decomposition matrix solver */ +#include "zbrent.h" /* 1 dimentional brent root search */ +#include "rand.h" /* Random number generators */ +#include "sobol.h" /* Sub-random vector generators */ +#include "aatree.h" /* Anderson balanced binary tree */ + +#endif /* NUMLIB_H */ diff --git a/numlib/numsup.c b/numlib/numsup.c new file mode 100644 index 0000000..7648a62 --- /dev/null +++ b/numlib/numsup.c @@ -0,0 +1,1539 @@ + +/* General Numerical routines support functions, */ +/* and common Argyll support functions. */ +/* (Perhaps these should be moved out of numlib at some stange ?) */ + +/* + * Copyright 1997 - 2010 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU GENERAL PUBLIC LICENSE Version 2 or later :- + * see the License2.txt file for licencing details. + */ + +#include <stdio.h> +#include <stdlib.h> +#include <math.h> +#include <string.h> +#include <limits.h> +#include <time.h> +#if defined (NT) +#define WIN32_LEAN_AND_MEAN +#include <windows.h> +#endif +#ifdef UNIX +#include <unistd.h> +#include <sys/param.h> +#include <pthread.h> +#endif + +#include "numsup.h" + +/* + * TODO: Should probably break all the non-numlib stuff out into + * a separate library, so that it can be ommitted. + * Or enhance it so that numerical callers of error() + * can get a callback on out of memory etc. ??? + * + */ + +/* Globals */ + +char *exe_path = "\000"; /* Directory executable resides in ('/' dir separator) */ +//char *error_program = "Unknown"; /* Name to report as responsible for an error */ + +static int g_log_init = 0; /* Initialised ? */ +extern a1log default_log; +extern a1log *g_log; + +/* Should Vector/Matrix Support functions return NULL on error, */ +/* or call error() ? */ +int ret_null_on_malloc_fail = 0; /* Call error() */ + +/******************************************************************/ +/* Executable path routine. Sets default error_program too. */ +/******************************************************************/ + +/* Pass in argv[0] from main() */ +/* Sets the error_program name too */ +void set_exe_path(char *argv0) { + int i; + + g_log->tag = argv0; + i = strlen(argv0); + if ((exe_path = malloc(i + 5)) == NULL) { + a1loge(g_log, 1, "set_exe_path: malloc %d bytes failed",i+5); + return; + } + strcpy(exe_path, argv0); + +#ifdef NT /* CMD.EXE doesn't give us the full path in argv[0] :-( */ + /* so we need to fix this */ + { + HMODULE mh; + char *tpath = NULL; + int pl; + + /* Add an .exe extension if it is missing */ + if (i < 4 || _stricmp(exe_path +i -4, ".exe") != 0) + strcat(exe_path, ".exe"); + + if ((mh = GetModuleHandle(exe_path)) == NULL) { + a1loge(g_log, 1, "set_exe_path: GetModuleHandle '%s' failed with%d", + exe_path,GetLastError()); + exe_path[0] = '\000'; + return; + } + + /* Retry until we don't truncate the returned path */ + for (pl = 100; ; pl *= 2) { + if (tpath != NULL) + free(tpath); + if ((tpath = malloc(pl)) == NULL) { + a1loge(g_log, 1, "set_exe_path: malloc %d bytes failed",pl); + exe_path[0] = '\000'; + return; + } + if ((i = GetModuleFileName(mh, tpath, pl)) == 0) { + a1loge(g_log, 1, "set_exe_path: GetModuleFileName '%s' failed with%d", + tpath,GetLastError()); + exe_path[0] = '\000'; + return; + } + if (i < pl) /* There was enough space */ + break; + } + free(exe_path); + exe_path = tpath; + + /* Convert from MSWindows to UNIX file separator convention */ + for (i = 0; ;i++) { + if (exe_path[i] == '\000') + break; + if (exe_path[i] == '\\') + exe_path[i] = '/'; + } + } +#else /* Neither does UNIX */ + + if (*exe_path != '/') { /* Not already absolute */ + char *p, *cp; + if (strchr(exe_path, '/') != 0) { /* relative path */ + cp = ".:"; /* Fake a relative PATH */ + } else { + cp = getenv("PATH"); + } + if (cp != NULL) { + int found = 0; + while((p = strchr(cp,':')) != NULL + || *cp != '\000') { + char b1[PATH_MAX], b2[PATH_MAX]; + int ll; + if (p == NULL) + ll = strlen(cp); + else + ll = p - cp; + if ((ll + 1 + strlen(exe_path) + 1) > PATH_MAX) { + a1loge(g_log, 1, "set_exe_path: Search path exceeds PATH_MAX"); + exe_path[0] = '\000'; + return; + } + strncpy(b1, cp, ll); /* Element of path to search */ + b1[ll] = '\000'; + strcat(b1, "/"); + strcat(b1, exe_path); /* Construct path */ + if (realpath(b1, b2)) { + if (access(b2, 0) == 0) { /* See if exe exits */ + found = 1; + free(exe_path); + if ((exe_path = malloc(strlen(b2)+1)) == NULL) { + a1loge(g_log, 1, "set_exe_path: malloc %d bytes failed",strlen(b2)+1); + exe_path[0] = '\000'; + return; + } + strcpy(exe_path, b2); + break; + } + } + if (p == NULL) + break; + cp = p + 1; + } + if (found == 0) + exe_path[0] = '\000'; + } + } +#endif + /* strip the executable path to the base */ + for (i = strlen(exe_path)-1; i >= 0; i--) { + if (exe_path[i] == '/') { + char *tpath; + if ((tpath = malloc(strlen(exe_path + i))) == NULL) { + a1loge(g_log, 1, "set_exe_path: malloc %d bytes failed",strlen(exe_path + i)); + exe_path[0] = '\000'; + return; + } + strcpy(tpath, exe_path + i + 1); + g_log->tag = tpath; /* Set g_log->tag to base name */ + exe_path[i+1] = '\000'; /* (The malloc never gets free'd) */ + break; + } + } + /* strip off any .exe from the g_log->tag to be more readable */ + i = strlen(g_log->tag); + if (i >= 4 + && g_log->tag[i-4] == '.' + && (g_log->tag[i-3] == 'e' || g_log->tag[i-3] == 'E') + && (g_log->tag[i-2] == 'x' || g_log->tag[i-2] == 'X') + && (g_log->tag[i-1] == 'e' || g_log->tag[i-1] == 'E')) + g_log->tag[i-4] = '\000'; + +// a1logd(g_log, 1, "exe_path = '%s'\n",exe_path); +// a1logd(g_log, 1, "g_log->tag = '%s'\n",g_log->tag); +} + +/******************************************************************/ +/* Check "ARGYLL_NOT_INTERACTIVE" environment variable. */ +/******************************************************************/ + +/* Check if the "ARGYLL_NOT_INTERACTIVE" environment variable is */ +/* set, and set cr_char to '\n' if it is. */ + +int not_interactive = 0; +char cr_char = '\r'; + +void check_if_not_interactive() { + char *ev; + + if ((ev = getenv("ARGYLL_NOT_INTERACTIVE")) != NULL) { + not_interactive = 1; + cr_char = '\n'; + } else { + not_interactive = 0; + cr_char = '\r'; + } +} + +/******************************************************************/ +/* Default verbose/debug/error loger */ +/* It's values can be overridden to redirect these messages. */ +/******************************************************************/ + +#ifdef NT +# define A1LOG_LOCK(log) \ + if (g_log_init == 0) { \ + InitializeCriticalSection(&log->lock); \ + g_log_init = 1; \ + } \ + EnterCriticalSection(&log->lock) +# define A1LOG_UNLOCK(log) LeaveCriticalSection(&log->lock) +#endif +#ifdef UNIX +# define A1LOG_LOCK(log) \ + if (g_log_init == 0) { \ + pthread_mutex_init(&log->lock, NULL); \ + g_log_init = 1; \ + } \ + pthread_mutex_lock(&log->lock) +# define A1LOG_UNLOCK(log) pthread_mutex_unlock(&log->lock) +#endif + + +/* Default verbose logging function - print to stdtout */ +static void a1_default_v_log(void *cntx, a1log *p, char *fmt, va_list args) { + vfprintf(stdout, fmt, args); + fflush(stdout); +} + +/* Default debug & error logging function - print to stderr */ +static void a1_default_de_log(void *cntx, a1log *p, char *fmt, va_list args) { + vfprintf(stderr, fmt, args); + fflush(stderr); +} + +#define a1_default_d_log a1_default_de_log +#define a1_default_e_log a1_default_de_log + + +/* Global log */ +a1log default_log = { + 1, /* Refcount of 1 because this is not allocated or free'd */ + "argyll", /* Default tag */ + 0, /* Vebose off */ + 0, /* Debug off */ + NULL, /* Context */ + &a1_default_v_log, /* Default verbose to stdout */ + &a1_default_d_log, /* Default debug to stderr */ + &a1_default_e_log, /* Default error to stderr */ + 0, /* error code 0 */ + { '\000' } /* No error message */ +}; +a1log *g_log = &default_log; + +/* If log NULL, allocate a new log and return it, */ +/* otherwise increment reference count and return existing log, */ +/* exit() if malloc fails. */ +a1log *new_a1log( + a1log *log, /* Existing log to reference, NULL if none */ + int verb, /* Verbose level to set */ + int debug, /* Debug level to set */ + void *cntx, /* Function context value */ + /* Vebose log function to call - stdout if NULL */ + void (*logv)(void *cntx, a1log *p, char *fmt, va_list args), + /* Debug log function to call - stderr if NULL */ + void (*logd)(void *cntx, a1log *p, char *fmt, va_list args), + /* Warning/error Log function to call - stderr if NULL */ + void (*loge)(void *cntx, a1log *p, char *fmt, va_list args) +) { + if (log != NULL) { + log->refc++; + return log; + } + if ((log = (a1log *)calloc(sizeof(a1log), 1)) == NULL) { + a1loge(g_log, 1, "new_a1log: malloc of a1log failed, calling exit(1)\n"); + exit(1); + } + log->verb = verb; + log->debug = debug; + + log->cntx = cntx; + if (logv != NULL) + log->logv = logv; + else + log->logv = a1_default_v_log; + + if (logd != NULL) + log->logd = logd; + else + log->logd = a1_default_d_log; + + if (loge != NULL) + log->loge = loge; + else + log->loge = a1_default_e_log; + + log->errc = 0; + log->errm[0] = '\000'; + + return log; +} + +/* Same as above but set default functions */ +a1log *new_a1log_d(a1log *log) { + return new_a1log(log, 0, 0, NULL, NULL, NULL, NULL); +} + +/* Decrement reference count and free log. */ +/* Returns NULL */ +a1log *del_a1log(a1log *log) { + if (log != NULL) { + if (--log->refc <= 0) { +#ifdef NT + DeleteCriticalSection(&log->lock); +#endif +#ifdef UNIX + pthread_mutex_destroy(&log->lock); +#endif + free(log); + } + } + return NULL; +} + +/* Set the tag. Note that the tage string is NOT copied, just referenced */ +void a1log_tag(a1log *log, char *tag) { + log->tag = tag; +} + +/* Log a verbose message if level >= verb */ +void a1logv(a1log *log, int level, char *fmt, ...) { + if (log != NULL) { + if (log->verb >= level) { + va_list args; + + A1LOG_LOCK(log); + va_start(args, fmt); + log->logv(log->cntx, log, fmt, args); + va_end(args); + A1LOG_UNLOCK(log); + } + } +} + +/* Log a debug message if level >= debug */ +void a1logd(a1log *log, int level, char *fmt, ...) { + if (log != NULL) { + if (log->debug >= level) { + va_list args; + + A1LOG_LOCK(log); + va_start(args, fmt); + log->loge(log->cntx, log, fmt, args); + va_end(args); + A1LOG_UNLOCK(log); + } + } +} + +/* log a warning message to the verbose, debug and error output, */ +void a1logw(a1log *log, char *fmt, ...) { + if (log != NULL) { + va_list args; + + /* log to all the outputs, but only log once */ + A1LOG_LOCK(log); + va_start(args, fmt); + log->loge(log->cntx, log, fmt, args); + va_end(args); + A1LOG_UNLOCK(log); + if (log->logd != log->loge) { + A1LOG_LOCK(log); + va_start(args, fmt); + log->logd(log->cntx, log, fmt, args); + va_end(args); + A1LOG_UNLOCK(log); + } + if (log->logv != log->loge && log->logv != log->logd) { + A1LOG_LOCK(log); + va_start(args, fmt); + log->logv(log->cntx, log, fmt, args); + va_end(args); + A1LOG_UNLOCK(log); + } + } +} + +/* log an error message to the verbose, debug and error output, */ +/* and latch the error if it is the first. */ +/* ecode = system, icoms or instrument error */ +void a1loge(a1log *log, int ecode, char *fmt, ...) { + if (log != NULL) { + va_list args; + + if (log->errc == 0) { + A1LOG_LOCK(log); + log->errc = ecode; + va_start(args, fmt); + vsnprintf(log->errm, A1_LOG_BUFSIZE, fmt, args); + va_end(args); + A1LOG_UNLOCK(log); + } + va_start(args, fmt); + /* log to all the outputs, but only log once */ + A1LOG_LOCK(log); + va_start(args, fmt); + log->loge(log->cntx, log, fmt, args); + va_end(args); + A1LOG_UNLOCK(log); + if (log->logd != log->loge) { + A1LOG_LOCK(log); + va_start(args, fmt); + log->logd(log->cntx, log, fmt, args); + va_end(args); + A1LOG_UNLOCK(log); + } + if (log->logv != log->loge && log->logv != log->logd) { + A1LOG_LOCK(log); + va_start(args, fmt); + log->logv(log->cntx, log, fmt, args); + va_end(args); + A1LOG_UNLOCK(log); + } + } +} + +/* Unlatch an error message. */ +/* This just resets errc and errm */ +void a1logue(a1log *log) { + if (log != NULL) { + log->errc = 0; + log->errm[0] = '\000'; + } +} + +/******************************************************************/ +/* Default verbose/warning/error output routines */ +/* These fall through to, and can be re-director using the */ +/* above log class. */ +/******************************************************************/ + +/* Some utilities to allow us to format output to log functions */ +/* (Caller aquires lock) */ +static void g_logv(char *fmt, ...) { + va_list args; + va_start(args, fmt); + g_log->logv(g_log->cntx, g_log, fmt, args); + va_end(args); +} + +static void g_loge(char *fmt, ...) { + va_list args; + va_start(args, fmt); + g_log->loge(g_log->cntx, g_log, fmt, args); + va_end(args); +} + +void +verbose(int level, char *fmt, ...) { + if (level >= g_log->verb) { + va_list args; + + A1LOG_LOCK(g_log); + g_logv("%s: ",g_log->tag); + va_start(args, fmt); + g_log->logv(g_log->cntx, g_log, fmt, args); + va_end(args); + g_logv("\n"); + A1LOG_UNLOCK(g_log); + } +} + +void +warning(char *fmt, ...) { + va_list args; + + A1LOG_LOCK(g_log); + g_loge("%s: Warning - ",g_log->tag); + va_start(args, fmt); + g_log->loge(g_log->cntx, g_log, fmt, args); + va_end(args); + g_loge("\n"); + A1LOG_UNLOCK(g_log); +} + +ATTRIBUTE_NORETURN void +error(char *fmt, ...) { + va_list args; + + A1LOG_LOCK(g_log); + g_loge("%s: Error - ",g_log->tag); + va_start(args, fmt); + g_log->loge(g_log->cntx, g_log, fmt, args); + va_end(args); + g_loge("\n"); + A1LOG_UNLOCK(g_log); + + exit(1); +} + + +/******************************************************************/ +/* Numerical Recipes Vector/Matrix Support functions */ +/******************************************************************/ +/* Note the z suffix versions return zero'd vectors/matricies */ + +/* Double Vector malloc/free */ +double *dvector( +int nl, /* Lowest index */ +int nh /* Highest index */ +) { + double *v; + + if ((v = (double *) malloc((nh-nl+1) * sizeof(double))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in dvector()"); + } + return v-nl; +} + +double *dvectorz( +int nl, /* Lowest index */ +int nh /* Highest index */ +) { + double *v; + + if ((v = (double *) calloc(nh-nl+1, sizeof(double))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in dvector()"); + } + return v-nl; +} + +void free_dvector( +double *v, +int nl, /* Lowest index */ +int nh /* Highest index */ +) { + if (v == NULL) + return; + + free((char *) (v+nl)); +} + +/* --------------------- */ +/* 2D Double vector malloc/free */ +double **dmatrix( +int nrl, /* Row low index */ +int nrh, /* Row high index */ +int ncl, /* Col low index */ +int nch /* Col high index */ +) { + int i; + int rows, cols; + double **m; + + if (nrh < nrl) /* Prevent failure for 0 dimension */ + nrh = nrl; + if (nch < ncl) + nch = ncl; + + rows = nrh - nrl + 1; + cols = nch - ncl + 1; + + if ((m = (double **) malloc((rows + 1) * sizeof(double *))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in dmatrix(), pointers"); + } + m -= nrl; /* Offset to nrl */ + m += 1; /* Make nrl-1 pointer to main allocation, in case rows get swaped */ + + if ((m[nrl-1] = (double *) malloc(rows * cols * sizeof(double))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in dmatrix(), array"); + } + + m[nrl] = m[nrl-1] - ncl; /* Set first row address, offset to ncl */ + for(i = nrl+1; i <= nrh; i++) /* Set subsequent row addresses */ + m[i] = m[i-1] + cols; + + return m; +} + +double **dmatrixz( +int nrl, /* Row low index */ +int nrh, /* Row high index */ +int ncl, /* Col low index */ +int nch /* Col high index */ +) { + int i; + int rows, cols; + double **m; + + if (nrh < nrl) /* Prevent failure for 0 dimension */ + nrh = nrl; + if (nch < ncl) + nch = ncl; + + rows = nrh - nrl + 1; + cols = nch - ncl + 1; + + if ((m = (double **) malloc((rows + 1) * sizeof(double *))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in dmatrix(), pointers"); + } + m -= nrl; /* Offset to nrl */ + m += 1; /* Make nrl-1 pointer to main allocation, in case rows get swaped */ + + if ((m[nrl-1] = (double *) calloc(rows * cols, sizeof(double))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in dmatrix(), array"); + } + + m[nrl] = m[nrl-1] - ncl; /* Set first row address, offset to ncl */ + for(i = nrl+1; i <= nrh; i++) /* Set subsequent row addresses */ + m[i] = m[i-1] + cols; + + return m; +} + +void free_dmatrix( +double **m, +int nrl, +int nrh, +int ncl, +int nch +) { + if (m == NULL) + return; + + if (nrh < nrl) /* Prevent failure for 0 dimension */ + nrh = nrl; + if (nch < ncl) + nch = ncl; + + free((char *)(m[nrl-1])); + free((char *)(m+nrl-1)); +} + +/* --------------------- */ +/* 2D diagonal half matrix vector malloc/free */ +/* A half matrix must have equal rows and columns, */ +/* and the column address must always be >= than the row. */ +double **dhmatrix( +int nrl, /* Row low index */ +int nrh, /* Row high index */ +int ncl, /* Col low index */ +int nch /* Col high index */ +) { + int i, j; + int rows, cols; + double **m; + + if (nrh < nrl) /* Prevent failure for 0 dimension */ + nrh = nrl; + if (nch < ncl) + nch = ncl; + + rows = nrh - nrl + 1; + cols = nch - ncl + 1; + + if (rows != cols) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("dhmatrix() given unequal rows and columns"); + } + + if ((m = (double **) malloc((rows + 1) * sizeof(double *))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in dhmatrix(), pointers"); + } + m -= nrl; /* Offset to nrl */ + m += 1; /* Make nrl-1 pointer to main allocation, in case rows get swaped */ + + if ((m[nrl-1] = (double *) malloc((rows * rows + rows)/2 * sizeof(double))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in dhmatrix(), array"); + } + + m[nrl] = m[nrl-1] - ncl; /* Set first row address, offset to ncl */ + for(i = nrl+1, j = 1; i <= nrh; i++, j++) /* Set subsequent row addresses */ + m[i] = m[i-1] + j; /* Start with 1 entry and increment */ + + return m; +} + +double **dhmatrixz( +int nrl, /* Row low index */ +int nrh, /* Row high index */ +int ncl, /* Col low index */ +int nch /* Col high index */ +) { + int i, j; + int rows, cols; + double **m; + + if (nrh < nrl) /* Prevent failure for 0 dimension */ + nrh = nrl; + if (nch < ncl) + nch = ncl; + + rows = nrh - nrl + 1; + cols = nch - ncl + 1; + + if (rows != cols) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("dhmatrix() given unequal rows and columns"); + } + + if ((m = (double **) malloc((rows + 1) * sizeof(double *))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in dhmatrix(), pointers"); + } + m -= nrl; /* Offset to nrl */ + m += 1; /* Make nrl-1 pointer to main allocation, in case rows get swaped */ + + if ((m[nrl-1] = (double *) calloc((rows * rows + rows)/2, sizeof(double))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in dhmatrix(), array"); + } + + m[nrl] = m[nrl-1] - ncl; /* Set first row address, offset to ncl */ + for(i = nrl+1, j = 1; i <= nrh; i++, j++) /* Set subsequent row addresses */ + m[i] = m[i-1] + j; /* Start with 1 entry and increment */ + + return m; +} + +void free_dhmatrix( +double **m, +int nrl, +int nrh, +int ncl, +int nch +) { + if (m == NULL) + return; + + if (nrh < nrl) /* Prevent failure for 0 dimension */ + nrh = nrl; + if (nch < ncl) + nch = ncl; + + free((char *)(m[nrl-1])); + free((char *)(m+nrl-1)); +} + +/* --------------------- */ +/* 2D vector copy */ +void copy_dmatrix( +double **dst, +double **src, +int nrl, /* Row low index */ +int nrh, /* Row high index */ +int ncl, /* Col low index */ +int nch /* Col high index */ +) { + int i, j; + for (j = nrl; j <= nrh; j++) + for (i = ncl; i <= nch; i++) + dst[j][i] = src[j][i]; +} + +/* Copy a matrix to a 3x3 standard C array */ +void copy_dmatrix_to3x3( +double dst[3][3], +double **src, +int nrl, /* Row low index */ +int nrh, /* Row high index */ +int ncl, /* Col low index */ +int nch /* Col high index */ +) { + int i, j; + if ((nrh - nrl) > 2) + nrh = nrl + 2; + if ((nch - ncl) > 2) + nch = ncl + 2; + for (j = nrl; j <= nrh; j++) + for (i = ncl; i <= nch; i++) + dst[j][i] = src[j][i]; +} + +/* -------------------------------------------------------------- */ +/* Convert standard C type 2D array into an indirect referenced array */ +double **convert_dmatrix( +double *a, /* base address of normal C array, ie &a[0][0] */ +int nrl, /* Row low index */ +int nrh, /* Row high index */ +int ncl, /* Col low index */ +int nch /* Col high index */ +) { + int i, j, nrow = nrh-nrl+1, ncol = nch-ncl+1; + double **m; + + /* Allocate pointers to rows */ + if ((m = (double **) malloc(nrow * sizeof(double*))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in convert_dmatrix()"); + } + + m -= nrl; + + m[nrl] = a - ncl; + for(i=1, j = nrl+1; i < nrow; i++, j++) + m[j] = m[j-1] + ncol; + /* return pointer to array of pointers */ + return m; +} + +/* Free the indirect array reference (but not array) */ +void free_convert_dmatrix( +double **m, +int nrl, +int nrh, +int ncl, +int nch +) { + if (m == NULL) + return; + + free((char*) (m+nrl)); +} + +/* -------------------------- */ +/* Float vector malloc/free */ +float *fvector( +int nl, /* Lowest index */ +int nh /* Highest index */ +) { + float *v; + + if ((v = (float *) malloc((nh-nl+1) * sizeof(float))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in fvector()"); + } + return v-nl; +} + +float *fvectorz( +int nl, /* Lowest index */ +int nh /* Highest index */ +) { + float *v; + + if ((v = (float *) calloc(nh-nl+1, sizeof(float))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in fvector()"); + } + return v-nl; +} + +void free_fvector( +float *v, +int nl, /* Lowest index */ +int nh /* Highest index */ +) { + if (v == NULL) + return; + + free((char *) (v+nl)); +} + +/* --------------------- */ +/* 2D Float matrix malloc/free */ +float **fmatrix( +int nrl, /* Row low index */ +int nrh, /* Row high index */ +int ncl, /* Col low index */ +int nch /* Col high index */ +) { + int i; + int rows, cols; + float **m; + + if (nrh < nrl) /* Prevent failure for 0 dimension */ + nrh = nrl; + if (nch < ncl) + nch = ncl; + + rows = nrh - nrl + 1; + cols = nch - ncl + 1; + + if ((m = (float **) malloc((rows + 1) * sizeof(float *))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in dmatrix(), pointers"); + } + m -= nrl; /* Offset to nrl */ + m += 1; /* Make nrl-1 pointer to main allocation, in case rows get swaped */ + + if ((m[nrl-1] = (float *) malloc(rows * cols * sizeof(float))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in dmatrix(), array"); + } + + m[nrl] = m[nrl-1] - ncl; /* Set first row address, offset to ncl */ + for(i = nrl+1; i <= nrh; i++) /* Set subsequent row addresses */ + m[i] = m[i-1] + cols; + + return m; +} + +float **fmatrixz( +int nrl, /* Row low index */ +int nrh, /* Row high index */ +int ncl, /* Col low index */ +int nch /* Col high index */ +) { + int i; + int rows, cols; + float **m; + + if (nrh < nrl) /* Prevent failure for 0 dimension */ + nrh = nrl; + if (nch < ncl) + nch = ncl; + + rows = nrh - nrl + 1; + cols = nch - ncl + 1; + + if ((m = (float **) malloc((rows + 1) * sizeof(float *))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in dmatrix(), pointers"); + } + m -= nrl; /* Offset to nrl */ + m += 1; /* Make nrl-1 pointer to main allocation, in case rows get swaped */ + + if ((m[nrl-1] = (float *) calloc(rows * cols, sizeof(float))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in dmatrix(), array"); + } + + m[nrl] = m[nrl-1] - ncl; /* Set first row address, offset to ncl */ + for(i = nrl+1; i <= nrh; i++) /* Set subsequent row addresses */ + m[i] = m[i-1] + cols; + + return m; +} + +void free_fmatrix( +float **m, +int nrl, +int nrh, +int ncl, +int nch +) { + if (m == NULL) + return; + + if (nrh < nrl) /* Prevent failure for 0 dimension */ + nrh = nrl; + if (nch < ncl) + nch = ncl; + + free((char *)(m[nrl-1])); + free((char *)(m+nrl-1)); +} + +/* ------------------ */ +/* Integer vector malloc/free */ +int *ivector( +int nl, /* Lowest index */ +int nh /* Highest index */ +) { + int *v; + + if ((v = (int *) malloc((nh-nl+1) * sizeof(int))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in ivector()"); + } + return v-nl; +} + +int *ivectorz( +int nl, /* Lowest index */ +int nh /* Highest index */ +) { + int *v; + + if ((v = (int *) calloc(nh-nl+1, sizeof(int))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in ivector()"); + } + return v-nl; +} + +void free_ivector( +int *v, +int nl, /* Lowest index */ +int nh /* Highest index */ +) { + if (v == NULL) + return; + + free((char *) (v+nl)); +} + + +/* ------------------------------ */ +/* 2D integer matrix malloc/free */ + +int **imatrix( +int nrl, /* Row low index */ +int nrh, /* Row high index */ +int ncl, /* Col low index */ +int nch /* Col high index */ +) { + int i; + int rows, cols; + int **m; + + if (nrh < nrl) /* Prevent failure for 0 dimension */ + nrh = nrl; + if (nch < ncl) + nch = ncl; + + rows = nrh - nrl + 1; + cols = nch - ncl + 1; + + if ((m = (int **) malloc((rows + 1) * sizeof(int *))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in imatrix(), pointers"); + } + m -= nrl; /* Offset to nrl */ + m += 1; /* Make nrl-1 pointer to main allocation, in case rows get swaped */ + + if ((m[nrl-1] = (int *) malloc(rows * cols * sizeof(int))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in imatrix(), array"); + } + + m[nrl] = m[nrl-1] - ncl; /* Set first row address, offset to ncl */ + for(i = nrl+1; i <= nrh; i++) /* Set subsequent row addresses */ + m[i] = m[i-1] + cols; + + return m; +} + +int **imatrixz( +int nrl, /* Row low index */ +int nrh, /* Row high index */ +int ncl, /* Col low index */ +int nch /* Col high index */ +) { + int i; + int rows, cols; + int **m; + + if (nrh < nrl) /* Prevent failure for 0 dimension */ + nrh = nrl; + if (nch < ncl) + nch = ncl; + + rows = nrh - nrl + 1; + cols = nch - ncl + 1; + + if ((m = (int **) malloc((rows + 1) * sizeof(int *))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in imatrix(), pointers"); + } + m -= nrl; /* Offset to nrl */ + m += 1; /* Make nrl-1 pointer to main allocation, in case rows get swaped */ + + if ((m[nrl-1] = (int *) calloc(rows * cols, sizeof(int))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in imatrix(), array"); + } + + m[nrl] = m[nrl-1] - ncl; /* Set first row address, offset to ncl */ + for(i = nrl+1; i <= nrh; i++) /* Set subsequent row addresses */ + m[i] = m[i-1] + cols; + + return m; +} + +void free_imatrix( +int **m, +int nrl, +int nrh, +int ncl, +int nch +) { + if (m == NULL) + return; + + if (nrh < nrl) /* Prevent failure for 0 dimension */ + nrh = nrl; + if (nch < ncl) + nch = ncl; + + free((char *)(m[nrl-1])); + free((char *)(m+nrl-1)); +} + +/* ------------------ */ +/* Short vector malloc/free */ +short *svector( +int nl, /* Lowest index */ +int nh /* Highest index */ +) { + short *v; + + if ((v = (short *) malloc((nh-nl+1) * sizeof(short))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in svector()"); + } + return v-nl; +} + +short *svectorz( +int nl, /* Lowest index */ +int nh /* Highest index */ +) { + short *v; + + if ((v = (short *) calloc(nh-nl+1, sizeof(short))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in svector()"); + } + return v-nl; +} + +void free_svector( +short *v, +int nl, /* Lowest index */ +int nh /* Highest index */ +) { + if (v == NULL) + return; + + free((char *) (v+nl)); +} + + +/* ------------------------------ */ +/* 2D short vector malloc/free */ + +short **smatrix( +int nrl, /* Row low index */ +int nrh, /* Row high index */ +int ncl, /* Col low index */ +int nch /* Col high index */ +) { + int i; + int rows, cols; + short **m; + + if (nrh < nrl) /* Prevent failure for 0 dimension */ + nrh = nrl; + if (nch < ncl) + nch = ncl; + + rows = nrh - nrl + 1; + cols = nch - ncl + 1; + + if ((m = (short **) malloc((rows + 1) * sizeof(short *))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in smatrix(), pointers"); + } + m -= nrl; /* Offset to nrl */ + m += 1; /* Make nrl-1 pointer to main allocation, in case rows get swaped */ + + if ((m[nrl-1] = (short *) malloc(rows * cols * sizeof(short))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in smatrix(), array"); + } + + m[nrl] = m[nrl-1] - ncl; /* Set first row address, offset to ncl */ + for(i = nrl+1; i <= nrh; i++) /* Set subsequent row addresses */ + m[i] = m[i-1] + cols; + + return m; +} + +short **smatrixz( +int nrl, /* Row low index */ +int nrh, /* Row high index */ +int ncl, /* Col low index */ +int nch /* Col high index */ +) { + int i; + int rows, cols; + short **m; + + if (nrh < nrl) /* Prevent failure for 0 dimension */ + nrh = nrl; + if (nch < ncl) + nch = ncl; + + rows = nrh - nrl + 1; + cols = nch - ncl + 1; + + if ((m = (short **) malloc((rows + 1) * sizeof(short *))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in smatrix(), pointers"); + } + m -= nrl; /* Offset to nrl */ + m += 1; /* Make nrl-1 pointer to main allocation, in case rows get swaped */ + + if ((m[nrl-1] = (short *) calloc(rows * cols, sizeof(short))) == NULL) { + if (ret_null_on_malloc_fail) + return NULL; + else + error("Malloc failure in smatrix(), array"); + } + + m[nrl] = m[nrl-1] - ncl; /* Set first row address, offset to ncl */ + for(i = nrl+1; i <= nrh; i++) /* Set subsequent row addresses */ + m[i] = m[i-1] + cols; + + return m; +} + +void free_smatrix( +short **m, +int nrl, +int nrh, +int ncl, +int nch +) { + if (m == NULL) + return; + + if (nrh < nrl) /* Prevent failure for 0 dimension */ + nrh = nrl; + if (nch < ncl) + nch = ncl; + + free((char *)(m[nrl-1])); + free((char *)(m+nrl-1)); +} + +/***************************/ +/* Basic matrix operations */ +/***************************/ + +/* Transpose a 0 base matrix */ +void matrix_trans(double **d, double **s, int nr, int nc) { + int i, j; + + for (i = 0; i < nr; i++) { + for (j = 0; j < nc; j++) { + d[j][i] = s[i][j]; + } + } +} + +/* Multiply two 0 based matricies */ +/* Return nz on matching error */ +int matrix_mult( + double **d, int nr, int nc, + double **s1, int nr1, int nc1, + double **s2, int nr2, int nc2 +) { + int i, j, k; + + /* s1 and s2 must mesh */ + if (nc1 != nr2) + return 1; + + /* Output rows = s1 rows */ + if (nr != nr1) + return 2; + + /* Output colums = s2 columns */ + if (nc != nc2) + return 2; + + for (i = 0; i < nr1; i++) { + for (j = 0; j < nc2; j++) { + d[i][j] = 0.0; + for (k = 0; k < nc1; k++) { + d[i][j] += s1[i][k] * s2[k][j]; + } + } + } + + return 0; +} + +/* Diagnostic - print to g_log debug */ +void matrix_print(char *c, double **a, int nr, int nc) { + int i, j; + a1logd(g_log, 0, "%s, %d x %d\n",c,nr,nc); + + for (j = 0; j < nr; j++) { + a1logd(g_log, 0, " "); + for (i = 0; i < nc; i++) { + a1logd(g_log, 0, " %.2f",a[j][i]); + } + a1logd(g_log, 0, "\n"); + } +} + + +/*******************************************/ +/* Platform independent IEE754 conversions */ +/*******************************************/ + +/* Cast a native double to an IEEE754 encoded single precision value, */ +/* in a platform independent fashion. (ie. This works even */ +/* on the rare platforms that don't use IEEE 754 floating */ +/* point for their C implementation) */ +ORD32 doubletoIEEE754(double d) { + ORD32 sn = 0, ep = 0, ma; + ORD32 id; + + /* Convert double to IEEE754 single precision. */ + /* This would be easy if we're running on an IEEE754 architecture, */ + /* but isn't generally portable, so we use ugly code: */ + + if (d < 0.0) { + sn = 1; + d = -d; + } + if (d != 0.0) { + int ee; + ee = (int)floor(log(d)/log(2.0)); + if (ee < -126) /* Allow for denormalized */ + ee = -126; + d *= pow(0.5, (double)(ee - 23)); + ee += 127; + if (ee < 1) /* Too small */ + ee = 0; /* Zero or denormalised */ + else if (ee > 254) { /* Too large */ + ee = 255; /* Infinity */ + d = 0.0; + } + ep = ee; + } else { + ep = 0; /* Zero */ + } + ma = ((ORD32)d) & ((1 << 23)-1); + id = (sn << 31) | (ep << 23) | ma; + + return id; +} + +/* Cast a an IEEE754 encoded single precision value to a native double, */ +/* in a platform independent fashion. (ie. This works even */ +/* on the rare platforms that don't use IEEE 754 floating */ +/* point for their C implementation) */ +double IEEE754todouble(ORD32 ip) { + double op; + ORD32 sn = 0, ep = 0, ma; + + sn = (ip >> 31) & 0x1; + ep = (ip >> 23) & 0xff; + ma = ip & 0x7fffff; + + if (ep == 0) { /* Zero or denormalised */ + op = (double)ma/(double)(1 << 23); + op *= pow(2.0, (-126.0)); + } else { + op = (double)(ma | (1 << 23))/(double)(1 << 23); + op *= pow(2.0, (((int)ep)-127.0)); + } + if (sn) + op = -op; + return op; +} + +/* Cast a native double to an IEEE754 encoded double precision value, */ +/* in a platform independent fashion. (ie. This works even */ +/* on the rare platforms that don't use IEEE 754 floating */ +/* point for their C implementation) */ +ORD64 doubletoIEEE754_64(double d) { + ORD32 sn = 0, ep = 0; + ORD64 ma, id; + + /* Convert double to IEEE754 double precision. */ + /* This would be easy if we know we're running on an IEEE754 architecture, */ + /* but isn't generally portable, so we use ugly code: */ + + if (d < 0.0) { + sn = 1; + d = -d; + } + if (d != 0.0) { + int ee; + ee = (int)floor(log(d)/log(2.0)); + if (ee < -1022) /* Allow for denormalized */ + ee = -1022; + d *= pow(0.5, (double)(ee - 52)); + ee += 1023; /* Exponent bias */ + if (ee < 1) /* Too small */ + ee = 0; /* Zero or denormalised */ + else if (ee > 2046) { /* Too large */ + ee = 2047; /* Infinity */ + d = 0.0; + } + ep = ee; + } else { + ep = 0; /* Zero */ + } + ma = ((ORD64)d) & (((ORD64)1 << 52)-1); + id = ((ORD64)sn << 63) | ((ORD64)ep << 52) | ma; + + return id; +} + +/* Cast a an IEEE754 encode double precision value to a native double, */ +/* in a platform independent fashion. (ie. This works even */ +/* on the rare platforms that don't use IEEE 754 floating */ +/* point for their C implementation) */ +double IEEE754_64todouble(ORD64 ip) { + double op; + ORD32 sn = 0, ep = 0; + INR64 ma; + + sn = (ip >> 63) & 0x1; + ep = (ip >> 52) & 0x7ff; + ma = ip & (((INR64)1 << 52)-1); + + if (ep == 0) { /* Zero or denormalised */ + op = (double)ma/(double)((INR64)1 << 52); + op *= pow(2.0, -1022.0); + } else { + op = (double)(ma | ((INR64)1 << 52))/(double)((INR64)1 << 52); + op *= pow(2.0, (((int)ep)-1023.0)); + } + if (sn) + op = -op; + return op; +} + +/* Return a string representation of a 32 bit ctime. */ +/* A static buffer is used. There is no \n at the end */ +char *ctime_32(const INR32 *timer) { + char *rv; +#if defined(_MSC_VER) && __MSVCRT_VERSION__ >= 0x0601 + rv = _ctime32((const __time32_t *)timer); +#else + time_t timerv = (time_t) *timer; /* May case to 64 bit */ + rv = ctime(&timerv); +#endif + + if (rv != NULL) + rv[strlen(rv)-1] = '\000'; + + return rv; +} + +/* Return a string representation of a 64 bit ctime. */ +/* A static buffer is used. There is no \n at the end */ +char *ctime_64(const INR64 *timer) { + char *rv; +#if defined(_MSC_VER) && __MSVCRT_VERSION__ >= 0x0601 + rv = _ctime64((const __time64_t *)timer); +#else + time_t timerv; + + if (sizeof(time_t) == 4 && *timer > 0x7fffffff) + return NULL; + timerv = (time_t) *timer; /* May truncate to 32 bits */ + rv = ctime(&timerv); +#endif + + if (rv != NULL) + rv[strlen(rv)-1] = '\000'; + + return rv; +} diff --git a/numlib/numsup.h b/numlib/numsup.h new file mode 100644 index 0000000..7db7676 --- /dev/null +++ b/numlib/numsup.h @@ -0,0 +1,380 @@ +#ifndef NUMSUP_H + +/* Numerical routine general support declarations */ + +/* + * Copyright 2000-2010 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU GENERAL PUBLIC LICENSE Version 2 or later :- + * see the License2.txt file for licencing details. + */ + +#include <stdio.h> +#include <stdlib.h> +#include <stdarg.h> +#include <string.h> +#include <float.h> +#include <math.h> + +#ifdef NT +#include <basetsd.h> /* So jpg header doesn't define INT32 */ +# if !defined(_WIN32_WINNT) || _WIN32_WINNT < 0x0501 +# undef _WIN32_WINNT +# define _WIN32_WINNT 0x0501 +# endif +# define WIN32_LEAN_AND_MEAN +# include <windows.h> +#endif +#ifdef UNIX +# include <pthread.h> +#endif + +#ifdef __cplusplus + extern "C" { +#endif + +/* =========================================================== */ +/* Should this go in spectro/conv.h ?? */ +/* =========================================================== */ +/* Platform specific primitive defines. */ +/* This really needs checking for each different platform. */ +/* Using C99 and MSC covers a lot of cases, */ +/* and the fallback default is pretty reliable with modern compilers and machines. */ +/* (duplicated in icc.h) */ + +#if (__STDC_VERSION__ >= 199901L) /* C99 */ + +#include <stdint.h> + +#define INR8 int8_t /* 8 bit signed */ +#define INR16 int16_t /* 16 bit signed */ +#define INR32 int32_t /* 32 bit signed */ +#define INR64 int64_t /* 64 bit signed - not used in icclib */ +#define ORD8 uint8_t /* 8 bit unsigned */ +#define ORD16 uint16_t /* 16 bit unsigned */ +#define ORD32 uint32_t /* 32 bit unsigned */ +#define ORD64 uint64_t /* 64 bit unsigned - not used in icclib */ + +#define PNTR intptr_t + +/* printf format precision specifier */ +#define PF64PREC "ll" + +/* Constant precision specifier */ +#define CF64PREC "LL" + +#ifndef ATTRIBUTE_NORETURN +# define ATTRIBUTE_NORETURN __declspec(noreturn) +#endif + +#else /* !__STDC_VERSION__ */ +#ifdef _MSC_VER + +#define INR8 __int8 /* 8 bit signed */ +#define INR16 __int16 /* 16 bit signed */ +#define INR32 __int32 /* 32 bit signed */ +#define INR64 __int64 /* 64 bit signed - not used in icclib */ +#define ORD8 unsigned __int8 /* 8 bit unsigned */ +#define ORD16 unsigned __int16 /* 16 bit unsigned */ +#define ORD32 unsigned __int32 /* 32 bit unsigned */ +#define ORD64 unsigned __int64 /* 64 bit unsigned - not used in icclib */ + +#define PNTR UINT_PTR + +#define vsnprintf _vsnprintf + +/* printf format precision specifier */ +#define PF64PREC "I64" + +/* Constant precision specifier */ +#define CF64PREC "LL" + +#ifndef ATTRIBUTE_NORETURN +# define ATTRIBUTE_NORETURN __declspec(noreturn) +#endif + +#else /* !_MSC_VER */ + +/* The following works on a lot of modern systems, including */ +/* LP64 model 64 bit modes */ + +#define INR8 signed char /* 8 bit signed */ +#define INR16 signed short /* 16 bit signed */ +#define INR32 signed int /* 32 bit signed */ +#define ORD8 unsigned char /* 8 bit unsigned */ +#define ORD16 unsigned short /* 16 bit unsigned */ +#define ORD32 unsigned int /* 32 bit unsigned */ + +#ifdef __GNUC__ +#define INR64 long long /* 64 bit signed - not used in icclib */ +#define ORD64 unsigned long long /* 64 bit unsigned - not used in icclib */ + +/* printf format precision specifier */ +#define PF64PREC "ll" + +/* Constant precision specifier */ +#define CF64PREC "LL" + +#ifndef ATTRIBUTE_NORETURN +#define ATTRIBUTE_NORETURN __attribute__((noreturn)) +#endif + +#endif /* __GNUC__ */ +/* =========================================================== */ + +#define PNTR unsigned long + +#endif /* !_MSC_VER */ +#endif /* !__STDC_VERSION__ */ +/* Some default math limits and constants */ +#ifndef DBL_EPSILON +#define DBL_EPSILON 2.2204460492503131e-016 /* 1.0+DBL_EPSILON != 1.0 */ +#endif +#ifndef DBL_MIN +#define DBL_MIN 2.2250738585072014e-308 /* IEEE 64 bit min value */ +#endif +#ifndef DBL_MAX +#define DBL_MAX 1.7976931348623158e+308 /* IEEE 64 bit max value */ +#endif +#ifndef DBL_PI +#define DBL_PI 3.1415926535897932384626433832795 +#endif + +/* =========================================================== */ +/* General verbose, debug, warning and error logging object and functions */ + +/* + + Verbose is simply informational for the end user during normal + operation, typically on stdout or in a text information log. + This will be logged to the verbose log stream. Level: + + 0 = Off + 1 = Brief progress and informational messages + 2 = Much more verbose information + 3+ = Very verbose detail + + Debug output is for the developer, to trace what has happened so + as to help diagnose a fault. This will be logged to the debug log stream. + Level: + + 0 = Off + 1 = Brief debug info and any internal errors that may be significant + if something subsequently fails. + 1-5 = Application internals at increasing level of detail + 2-6 = Driver level.(overlaps app & coms) + 6-7 = high level communications + 8-9 = low level communications. + + Warning is a serious internal fault that is going to be ignored at the + point it is noticed, but may explain any unexpected behaviour. + It will be reported to the vebose, debug and error log streams. + + An error is something that is regarded as fatal at the point it + is noticed. It will be reported to the vebose, debug and error log + streams. The error code and error message will be latched within the + log object if it is the first error logged. It can (theoretically) be + treated as a warning at a higher calling level by calling + a1logue (unlatch error) to reset the error code and message. + + */ + +#define A1_LOG_BUFSIZE 500 + + +struct _a1log { + int refc; /* Reference count */ + char *tag; /* Optional tag name */ + int verb; /* Current verbosity level (public) */ + int debug; /* Current debug level (public) */ + + void *cntx; /* Context to provide to log functions */ + void (*logv)(void *cntx, struct _a1log *p, char *fmt, va_list args); + /* Implementation of log verbose */ + void (*logd)(void *cntx, struct _a1log *p, char *fmt, va_list args); + /* Implementation of log debug */ + void (*loge)(void *cntx, struct _a1log *p, char *fmt, va_list args); + /* Implementation of log warning/error */ + + int errc; /* error code */ + char errm[A1_LOG_BUFSIZE]; /* error message (public) */ + +#ifdef NT + CRITICAL_SECTION lock; +#endif +#ifdef UNIX + pthread_mutex_t lock; +#endif +}; typedef struct _a1log a1log; + + + +/* If log NULL, allocate a new log and return it, */ +/* otherwise increment reference count and return existing log, */ +/* exit() if malloc fails. */ +a1log *new_a1log( + a1log *log, /* Existing log to reference, NULL if none */ + int verb, /* Verbose level to set */ + int debug, /* Debug level to set */ + void *cntx, /* Function context value */ + /* Debug log function to call - stdout if NULL */ + void (*logv)(void *cntx, a1log *p, char *fmt, va_list args), + /* Debug log function to call - stderr if NULL */ + void (*logd)(void *cntx, a1log *p, char *fmt, va_list args), + /* Warning/error Log function to call - stderr if NULL */ + void (*loge)(void *cntx, a1log *p, char *fmt, va_list args) +); + +/* Same as above but set default functions */ +a1log *new_a1log_d(a1log *log); + +/* Decrement reference count and free log. */ +/* Returns NULL */ +a1log *del_a1log(a1log *log); + +/* Set the tag. Note that the tage string is NOT copied, just referenced */ +void a1log_tag(a1log *log, char *tag); + +/* Log a verbose message if level >= verb */ +void a1logv(a1log *log, int level, char *fmt, ...); + +/* Log a debug message if level >= debug */ +void a1logd(a1log *log, int level, char *fmt, ...); + +/* log a warning message to the error output */ +void a1logw(a1log *log, char *fmt, ...); + +/* log an error message, */ +/* ecode = system, icoms or instrument error */ +void a1loge(a1log *log, int ecode, char *fmt, ...); + +/* Unlatch an error message. */ +/* This resets errc and errm */ +void a1logue(a1log *log); + +/* =========================================================== */ +/* Globals used to hold certain information */ +extern char *exe_path; /* Path leading to executable, not including exe name itself. */ + /* Always uses '/' separator */ + /* Malloce'd - won't be freed on exit (intended leak) */ + +extern a1log *g_log; /* Default log */ + +/* These legacy functions that now call through to the default log */ +#define error_program g_log->tag +extern void set_exe_path(char *arg0); + +extern void ATTRIBUTE_NORETURN error(char *fmt, ...); +extern void warning(char *fmt, ...); +extern void verbose(int level, char *fmt, ...); + +extern int ret_null_on_malloc_fail; + +extern void check_if_not_interactive(); +extern int not_interactive; +extern char cr_char; + +/* =========================================================== */ + +/* Numerical recipes vector/matrix support functions */ +/* Note that the index arguments are the inclusive low and high values */ + +/* Double */ +double *dvector(int nl,int nh); +double *dvectorz(int nl,int nh); +void free_dvector(double *v,int nl,int nh); + +double **dmatrix(int nrl, int nrh, int ncl, int nch); +double **dmatrixz(int nrl, int nrh, int ncl, int nch); +void free_dmatrix(double **m, int nrl, int nrh, int ncl, int nch); + +double **dhmatrix(int nrl, int nrh, int ncl, int nch); +double **dhmatrixz(int nrl, int nrh, int ncl, int nch); +void free_dhmatrix(double **m, int nrl, int nrh, int ncl, int nch); + +void copy_dmatrix(double **dst, double **src, int nrl, int nrh, int ncl, int nch); +void copy_dmatrix_to3x3(double dst[3][3], double **src, int nrl, int nrh, int ncl, int nch); + +double **convert_dmatrix(double *a,int nrl,int nrh,int ncl,int nch); +void free_convert_dmatrix(double **m,int nrl,int nrh,int ncl,int nch); + + +/* Float */ +float *fvector(int nl,int nh); +float *fvectorz(int nl,int nh); +void free_fvector(float *v,int nl,int nh); + +float **fmatrix(int nrl, int nrh, int ncl, int nch); +float **fmatrixz(int nrl, int nrh, int ncl, int nch); +void free_fmatrix(float **m, int nrl, int nrh, int ncl, int nch); + + +/* Int */ +int *ivector(int nl,int nh); +int *ivectorz(int nl,int nh); +void free_ivector(int *v,int nl,int nh); + +int **imatrix(int nrl,int nrh,int ncl,int nch); +int **imatrixz(int nrl,int nrh,int ncl,int nch); +void free_imatrix(int **m,int nrl,int nrh,int ncl,int nch); + + +/* Short */ +short *svector(int nl, int nh); +short *svectorz(int nl, int nh); +void free_svector(short *v, int nl, int nh); + +short **smatrix(int nrl,int nrh,int ncl,int nch); +short **smatrixz(int nrl,int nrh,int ncl,int nch); +void free_smatrix(short **m,int nrl,int nrh,int ncl,int nch); + +/* ----------------------------------------------------------- */ +/* Basic matrix operations */ + +/* Transpose a 0 base matrix */ +void matrix_trans(double **d, double **s, int nr, int nc); + +/* Matrix multiply 0 based matricies */ +int matrix_mult( + double **d, int nr, int nc, + double **s1, int nr1, int nc1, + double **s2, int nr2, int nc2 +); + +/* Diagnostic */ +void matrix_print(char *c, double **a, int nr, int nc); + +/* =========================================================== */ + +/* Cast a native double to an IEEE754 encoded single precision value, */ +/* in a platform independent fashion. */ +ORD32 doubletoIEEE754(double d); + +/* Cast an IEEE754 encoded single precision value to a native double, */ +/* in a platform independent fashion. */ +double IEEE754todouble(ORD32 ip); + + +/* Cast a native double to an IEEE754 encoded double precision value, */ +/* in a platform independent fashion. */ +ORD64 doubletoIEEE754_64(double d); + +/* Cast an IEEE754 encoded double precision value to a native double, */ +/* in a platform independent fashion. */ +double IEEE754_64todouble(ORD64 ip); + +/* Return a string representation of a 32 bit ctime. */ +/* A static buffer is used. There is no \n at the end */ +char *ctime_32(const INR32 *timer); + +/* Return a string representation of a 64 bit ctime. */ +/* A static buffer is used. There is no \n at the end */ +char *ctime_64(const INR64 *timer); + +#ifdef __cplusplus + } +#endif + +#define NUMSUP_H +#endif /* NUMSUP_H */ diff --git a/numlib/powell.c b/numlib/powell.c new file mode 100644 index 0000000..5d2adac --- /dev/null +++ b/numlib/powell.c @@ -0,0 +1,691 @@ + +/* Multi-dimentional minizer using Powell or Conjugate Gradient methods */ +/* This is good for smoother, well behaved functions. */ + +/* Code is an original expression of the algorithms decsribed in */ +/* "Numerical Recipes in C", by W.H.Press, B.P.Flannery, */ +/* S.A.Teukolsky & W.T.Vetterling. */ + +/* + * Copyright 2000, 2006 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +/* TTBD: + Fix error handling to return status (malloc, excessive itters) + Create to "safe" library ? + Make standalone - ie remove numsup ? + */ + +/* + + Idea for improving progress accounting: + + count number of itterations already done (pitter) + estimate number yet needed (fitter) + progress = pitter/(pitter + fitter) + + Number yet needed estimated by progress of retval delta + againsth threshold target. + + ie fitters = (lastdel - curdel)/(curdel - stopth) + +*/ + +/* Note that all arrays are indexed from 0 */ + +#include "numsup.h" +#include "powell.h" + +#undef SLOPE_SANITY_CHECK /* exermental */ +#undef ABSTOL /* Make tollerance absolute */ +#undef DEBUG /* Some debugging printfs (not comprehensive) */ + +#ifdef DEBUG +#undef DBG +#define DBG(xxx) printf xxx ; +#else +#undef DBG +#define DBG(xxx) +#endif + +static double linmin(double p[], double xi[], int n, double ftol, + double (*func)(void *fdata, double tp[]), void *fdata); + +/* Standard interface for powell function */ +/* return 0 on sucess, 1 on failure due to excessive itterions */ +/* Result will be in cp */ +int powell( +double *rv, /* If not NULL, return the residual error */ +int di, /* Dimentionality */ +double cp[], /* Initial starting point */ +double s[], /* Size of initial search area */ +#ifdef ABSTOL +double ftol, /* Absolute tollerance of error change to stop on */ +#else +double ftol, /* Relative tollerance of error change to stop on */ +#endif +int maxit, /* Maximum iterations allowed */ +double (*func)(void *fdata, double tp[]), /* Error function to evaluate */ +void *fdata, /* Opaque data needed by function */ +void (*prog)(void *pdata, int perc), /* Optional progress percentage callback */ +void *pdata /* Opaque data needed by prog() */ +) { + int i; + double **dmtx; /* Direction vector */ + double *spt; /* Sarting point before exploring all the directions */ + double *xpt; /* Extrapolated point */ + double *svec; /* Search vector */ + int iter; + double retv; /* Returned function value at p */ + double stopth; /* Current stop threshold */ + double startdel = -1.0; /* Initial change in function value */ + double curdel; /* Current change in function value */ + int pc = 0; /* Percentage complete */ + + dmtx = dmatrixz(0, di-1, 0, di-1); /* Zero filled */ + spt = dvector(0,di-1); + xpt = dvector(0,di-1); + svec = dvector(0,di-1); + + /* Create initial direction matrix by */ + /* placing search start on diagonal */ + for (i = 0; i < di; i++) + dmtx[i][i] = s[i]; + + /* Save the starting point */ + for (i = 0; i < di; i++) + spt[i] = cp[i]; + + if (prog != NULL) /* Report initial progress */ + prog(pdata, pc); + + /* Initial function evaluation */ + retv = (*func)(fdata, cp); + +//printf("~1 ### initial retv = %f\n",retv); + /* Itterate untill we converge on a solution, or give up. */ + for (iter = 1; iter < maxit; iter++) { + int j; + double lretv; /* Last function return value */ + int ibig = 0; /* Index of biggest delta */ + double del = 0.0; /* Biggest function value decrease */ + double pretv; /* Previous function return value */ + + pretv = retv; /* Save return value at top of itteration */ + + /* Loop over all directions in the set */ + for (i = 0; i < di; i++) { + + DBG(("Looping over direction %d\n",i)) + + for (j = 0; j < di; j++) /* Extract this direction to make search vector */ + svec[j] = dmtx[j][i]; + +//printf("~1 ### chosen dir = %f %f\n", svec[0],svec[1]); + /* Minimize in that direction */ + lretv = retv; + retv = linmin(cp, svec, di, ftol, func, fdata); + + /* Record bigest function decrease, and dimension it occured on */ + if (fabs(lretv - retv) > del) { + del = fabs(lretv - retv); + ibig = i; + } + } + +//printf("~1 ### biggest change was dir %d by %f\n", ibig, del); + +#ifdef ABSTOL + stopth = ftol; /* Absolute tollerance */ +#else + stopth = ftol * 0.5 * (fabs(pretv) + fabs(retv) + DBL_EPSILON); +#endif + curdel = fabs(pretv - retv); + if (startdel < 0.0) { + startdel = curdel; + } else { + int tt; + tt = (int)(100.0 * pow((log(curdel) - log(startdel))/(log(stopth) - log(startdel)), 4.0) + 0.5); + if (tt > pc && tt < 100) { + pc = tt; + if (prog != NULL) /* Report initial progress */ + prog(pdata, pc); + } + + } + /* If we have had at least one change of direction and */ + /* reached a suitable tollerance, then finish */ + if (iter > 1 && curdel <= stopth) { +//printf("~1 ### stopping on itter %d because curdel %f <= stopth %f\n",iter, curdel,stopth); + DBG(("Reached stop tollerance because curdel %f <= stopth %f\n",curdel,stopth)) + break; + } + DBG(("Not stopping because curdel %f > stopth %f\n",curdel,stopth)) + +//printf("~1 ### recomputing direction\n"); + for (i = 0; i < di; i++) { + svec[i] = cp[i] - spt[i]; /* Average direction moved after minimization round */ + xpt[i] = cp[i] + svec[i]; /* Extrapolated point after round of minimization */ + spt[i] = cp[i]; /* New start point for next round */ + } +//printf("~1 ### new dir = %f %f\n", svec[0],svec[1]); + + /* Function value at extrapolated point */ + lretv = (*func)(fdata, xpt); + + if (lretv < pretv) { /* If extrapolation is an improvement */ + double t, t1, t2; + +//printf("~1 ### extrap is improvement\n"); + t1 = pretv - retv - del; + t2 = pretv - lretv; + t = 2.0 * (pretv -2.0 * retv + lretv) * t1 * t1 - del * t2 * t2; + if (t < 0.0) { +//printf("~1 ### move to min in new direction\n"); + /* Move to the minimum of the new direction */ + retv = linmin(cp, svec, di, ftol, func, fdata); + + for (i = 0; i < di; i++) { /* Save the new direction */ + dmtx[i][ibig] = svec[i]; /* by replacing best previous */ + } + } + } + } + +//printf("~1 iters = %d\n",iter); + /* Free up all the temporary vectors and matrix */ + free_dvector(svec,0,di-1); + free_dvector(xpt,0,di-1); + free_dvector(spt,0,di-1); + free_dmatrix(dmtx, 0, di-1, 0, di-1); + + if (prog != NULL) /* Report final progress */ + prog(pdata, 100); + + if (rv != NULL) + *rv = retv; + + if (iter < maxit) + return 0; + + DBG(("powell: returning 1 due to excessive itterations\n")) + return 1; /* Failed due to execessive itterations */ +} + +/* -------------------------------------- */ +/* Conjugate Gradient optimiser */ +/* return 0 on sucess, 1 on failure due to excessive itterions */ +/* Result will be in cp */ +/* Note that we could use gradient in line minimiser, */ +/* but haven't bothered yet. */ +int conjgrad( +double *rv, /* If not NULL, return the residual error */ +int di, /* Dimentionality */ +double cp[], /* Initial starting point */ +double s[], /* Size of initial search area */ +#ifdef ABSTOL +double ftol, /* Absolute tollerance of error change to stop on */ +#else +double ftol, /* Relative tollerance of error change to stop on */ +#endif +int maxit, /* Maximum iterations allowed */ +double (*func)(void *fdata, double tp[]), /* Error function to evaluate */ +double (*dfunc)(void *fdata, double dp[], double tp[]), /* Gradient function to evaluate */ +void *fdata, /* Opaque data needed by function */ +void (*prog)(void *pdata, int perc), /* Optional progress percentage callback */ +void *pdata /* Opaque data needed by prog() */ +) { + int i, iter; + double *svec; /* Search vector */ + double *gvec; /* G direction vector */ + double *hvec; /* H direction vector */ + double retv; /* Returned function value at p */ + double stopth; /* Current stop threshold */ + double startdel = -1.0; /* Initial change in function value */ + double curdel; /* Current change in function value */ + double svec_sca; /* initial svec scale factor */ + int pc = 0; /* Percentage complete */ + + svec = dvector(0,di-1); + gvec = dvector(0,di-1); + hvec = dvector(0,di-1); + + if (prog != NULL) /* Report initial progress */ + prog(pdata, pc); + + /* Initial function evaluation */ + retv = (*dfunc)(fdata, svec, cp); + + /* svec[] seems to be large after this. */ + /* Rescale it to conform to maximum of s[] */ + for (svec_sca = 0.0, i = 0; i < di; i++) { + if (fabs(svec[i]) > svec_sca) + svec_sca = fabs(svec[i]); + } + /* set scale so largest <= 1 */ + if (svec_sca < 1e-12) + svec_sca = 1.0; + else + svec_sca = 1.0/svec_sca; + +//printf("~1 ### initial dir = %f %f\n", svec[0],svec[1]); +//printf("~1 ### initial retv = %f\n",retv); + /* Initial vector setup */ + for (i = 0; i < di; i++) { + gvec[i] = hvec[i] = -svec[i]; /* Inverse gradient */ + svec[i] = s[i] * -svec[i] * svec_sca; /* Scale the search vector */ + } +//printf("~1 ### svec = %f %f\n", svec[0],svec[1]); + + /* Itterate untill we converge on a solution, or give up. */ + for (iter = 1; iter < maxit; iter++) { + double gamden, gamnum, gam; + double pretv; /* Previous function return value */ + + DBG(("conjrad: about to do linmin\n")) + pretv = retv; + retv = linmin(cp, svec, di, ftol, func, fdata); + +#ifdef ABSTOL + stopth = ftol; /* Absolute tollerance */ +#else + stopth = ftol * 0.5 * (fabs(pretv) + fabs(retv) + DBL_EPSILON); // Old code +#endif + curdel = fabs(pretv - retv); +//printf("~1 ### this retv = %f, pretv = %f, curdel = %f\n",retv,pretv,curdel); + if (startdel < 0.0) { + startdel = curdel; + } else { + int tt; + tt = (int)(100.0 * pow((log(curdel) - log(startdel))/(log(stopth) - log(startdel)), 4.0) + 0.5); + if (tt > pc && tt < 100) { + pc = tt; + if (prog != NULL) /* Report initial progress */ + prog(pdata, pc); + } + } + + /* If we have had at least one change of direction and */ + /* reached a suitable tollerance, then finish */ + if (iter > 1 && curdel <= stopth) { +//printf("~1 ### stopping on itter %d because curdel %f <= stopth %f\n",iter, curdel,stopth); + break; + } +//printf("~1 ### Not stopping on itter %d because curdel %f > stopth %f\n",iter, curdel,stopth); + + DBG(("conjrad: recomputing direction\n")) +//printf("~1 ### recomputing direction\n"); + (*dfunc)(fdata, svec, cp); /* (Don't use retv as it wrecks stop test) */ + +//printf("~1 ### pderiv = %f %f\n", svec[0],svec[1]); + /* Compute gamma */ + for (gamnum = gamden = 0.0, i = 0; i < di; i++) { + gamnum += svec[i] * (gvec[i] + svec[i]); + gamden += gvec[i] * gvec[i]; + } + +//printf("~1 ### gamnum = %f, gamden = %f\n", gamnum,gamden); + if (gamden == 0.0) { /* Gradient is exactly zero */ + DBG(("conjrad: gradient is exactly zero\n")) + break; + } + + gam = gamnum/gamden; + DBG(("conjrad: gamma = %f = %f/%f\n",gam,gamnum,gamden)) +//printf("~1 ### gvec[] = %f %f, gamma = %f, hvec = %f %f\n", gvec[0],gvec[1],gam,hvec[0],hvec[1]); + + /* Adjust seach direction */ + for (i = 0; i < di; i++) { + gvec[i] = -svec[i]; + svec[i] = hvec[i] = gvec[i] + gam * hvec[i]; + } + + /* svec[] seems to be large after this. */ + /* Rescale it to conform to maximum of s[] */ + for (svec_sca = 0.0, i = 0; i < di; i++) { + if (fabs(svec[i]) > svec_sca) + svec_sca = fabs(svec[i]); + } + /* set scale so largest <= 1 */ + if (svec_sca < 1e-12) + svec_sca = 1.0; + else + svec_sca = 1.0/svec_sca; + for (i = 0; i < di; i++) + svec[i] = svec[i] * s[i] * svec_sca; +//printf("~1 ### svec = %f %f\n", svec[0],svec[1]); + + } + /* Free up all the temporary vectors and matrix */ + free_dvector(hvec,0,di-1); + free_dvector(gvec,0,di-1); + free_dvector(svec,0,di-1); + + if (prog != NULL) /* Report final progress */ + prog(pdata, 100); + + if (rv != NULL) + *rv = retv; + +//printf("~1 ### done\n"); + + if (iter < maxit) + return 0; + + return 1; /* Failed due to execessive itterations */ +} + +/*------------------------------*/ +#define POWELL_GOLD 1.618034 +#define POWELL_CGOLD 0.3819660 +#define POWELL_MAXIT 100 + +/* Line bracketing and minimisation routine. */ +/* Return value at minimum. */ +static double linmin( +double cp[], /* Start point, and returned value */ +double xi[], /* Search vector */ +int di, /* Dimensionality */ +#ifdef ABSTOL +double ftol, /* Absolute tolerance to stop on */ +#else +double ftol, /* Relative tolerance to stop on */ +#endif +double (*func)(void *fdata, double tp[]), /* Error function to evaluate */ +void *fdata) /* Opaque data for func() */ +{ + int i; + double ax, xx, bx; /* Search vector multipliers */ + double af, xf, bf; /* Function values at those points */ + double *xt, XT[10]; /* Trial point */ + + if (di <= 10) + xt = XT; + else + xt = dvector(0, di-1); /* Vector for trial point */ + + /* -------------------------- */ + /* First bracket the solution */ + + DBG(("linmin: Bracketing solution\n")) + + /* The line is measured as startpoint + offset * search vector. */ + /* (Search isn't symetric, but it seems to depend on cp being */ + /* best current solution ?) */ + ax = 0.0; + for (i = 0; i < di; i++) + xt[i] = cp[i] + ax * xi[i]; + af = (*func)(fdata, xt); + + /* xx being vector offset 0.618 */ + xx = 1.0/POWELL_GOLD; + for (i = 0; i < di; i++) + xt[i] = cp[i] + xx * xi[i]; + xf = (*func)(fdata, xt); + + DBG(("linmin: Initial points a:%f:%f -> b:%f:%f\n",ax,af,xx,xf)) + + /* Fix it so that we are decreasing from point a -> x */ + if (xf > af) { + double tt; + tt = ax; ax = xx; xx = tt; + tt = af; af = xf; xf = tt; + } + DBG(("linmin: Ordered Initial points a:%f:%f -> b:%f:%f\n",ax,af,xx,xf)) + + bx = xx + POWELL_GOLD * (xx-ax); /* Guess b beyond a -> x */ + for (i = 0; i < di; i++) + xt[i] = cp[i] + bx * xi[i]; + bf = (*func)(fdata, xt); + + DBG(("linmin: Initial bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf)) + +#ifdef SLOPE_SANITY_CHECK + /* If we're not seeing a slope indicitive of progress */ + /* of order ftol, give up straight away */ + if (2000.0 * fabs(xf - bf) <= ftol * (fabs(xf) + fabs(bf)) + && 2000.0 * fabs(af - xf) <= ftol * (fabs(af) + fabs(xf))) { + DBG(("linmin: giving up because slope is too shallow\n")) + if (xt != XT) + free_dvector(xt,0,di-1); + + if (bf < xf) { + xf = bf; + xx = bx; + } + + /* Compute solution vector */ + for (i = 0; i < di; i++) + cp[i] += xx * xi[i]; + return xf; + } +#endif /* SLOPE_SANITY_CHECK */ + + /* While not bracketed */ + while (xf > bf) { + double ulim, ux, uf; + double tt, r, q; + +// DBG(("linmin: Not bracketed a:%f:%f x:%f%f b:%f:%f\n",ax,af,xx,xf,bx,bf)) + DBG(("linmin: Not bracketed because xf %f > bf %f\n",xf, bf)) + DBG((" ax = %f, xx = %f, bx = %f\n",ax,xx,bx)) + + /* Compute ux by parabolic interpolation from a, x & b */ + q = (xx - bx) * (xf - af); + r = (xx - ax) * (xf - bf); + tt = q - r; + if (tt >= 0.0 && tt < 1e-20) /* If +ve too small */ + tt = 1e-20; + else if (tt <= 0.0 && tt > -1e-20) /* If -ve too small */ + tt = -1e-20; + ux = xx - ((xx - bx) * q - (xx - ax) * r) / (2.0 * tt); + ulim = xx + 100.0 * (bx - xx); /* Extrapolation limit */ + +//printf("~1 ux = %f, ulim = %f\n",ux,ulim); + if ((xx - ux) * (ux - bx) > 0.0) { /* u is between x and b */ + + for (i = 0; i < di; i++) /* Evaluate u */ + xt[i] = cp[i] + ux * xi[i]; + uf = (*func)(fdata, xt); + +//printf("~1 u is between x and b, uf = %f\n",uf); + + if (uf < bf) { /* Minimum is between x and b */ +//printf("~1 min is between x and b\n"); + ax = xx; af = xf; + xx = ux; xf = uf; + break; + } else if (uf > xf) { /* Minimum is between a and u */ +//printf("~1 min is between a and u\n"); + bx = ux; bf = uf; + break; + } + + /* Parabolic fit didn't work, look further out in direction of b */ + ux = bx + POWELL_GOLD * (bx-xx); +//printf("~1 parabolic fit didn't work,look further in direction of b (%f)\n",ux); + + } else if ((bx - ux) * (ux - ulim) > 0.0) { /* u is between b and limit */ + for (i = 0; i < di; i++) /* Evaluate u */ + xt[i] = cp[i] + ux * xi[i]; + uf = (*func)(fdata, xt); + +//printf("~1 u is between b and limit uf = %f\n",uf); + if (uf > bf) { /* Minimum is between x and u */ +//printf("~1 min is between x and uf\n"); + ax = xx; af = xf; + xx = bx; xf = bf; + bx = ux; bf = uf; + break; + } + xx = bx; xf = bf; /* Continue looking */ + bx = ux; bf = uf; + ux = bx + POWELL_GOLD * (bx - xx); /* Test beyond b */ +//printf("~1 continue looking beyond b (%f)\n",ux); + + } else if ((ux - ulim) * (ulim - bx) >= 0.0) { /* u is beyond limit */ + ux = ulim; +//printf("~1 use limit\n"); + } else { /* u is to left side of x ? */ + ux = bx + POWELL_GOLD * (bx-xx); +//printf("~1 look gold beyond b (%f)\n",ux); + } + /* Evaluate u, and move into place at b */ + for (i = 0; i < di; i++) + xt[i] = cp[i] + ux * xi[i]; + uf = (*func)(fdata, xt); +//printf("~1 lookup ux %f value uf = %f\n",ux,uf); + ax = xx; af = xf; + xx = bx; xf = bf; + bx = ux; bf = uf; +//printf("~1 move along to the right (a<-x, x<-b, b-<u)\n"); + } + DBG(("linmin: Got bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf)) + /* Got bracketed minimum between a -> x -> b */ +//printf("~1 got bracketed minimum at %f (%f), %f (%f), %f (%f)\n",ax,af,xx,xf,bx,bf); + + /* --------------------------------------- */ + /* Now use brent minimiser bewteen a and b */ + { + /* a and b bracket solution */ + /* x is best function value so far */ + /* w is second best function value so far */ + /* v is previous second best, or third best */ + /* u is most recently tested point */ + double wx, vx, ux; /* Search vector multipliers */ + double wf, vf = 0.0, uf; /* Function values at those points */ + int iter; + double de = 0.0; /* Distance moved on previous step */ + double e = 0.0; /* Distance moved on 2nd previous step */ + + /* Make sure a and b are in ascending order */ + if (ax > bx) { + double tt; + tt = ax; ax = bx; bx = tt; + tt = af; af = bf; bf = tt; + } + + wx = vx = xx; /* Initial values of other center points */ + wf = xf = xf; + + for (iter = 1; iter <= POWELL_MAXIT; iter++) { + double mx = 0.5 * (ax + bx); /* m is center of bracket values */ +#ifdef ABSTOL + double tol1 = ftol; /* Absolute tollerance */ +#else + double tol1 = ftol * fabs(xx) + 1e-10; +#endif + double tol2 = 2.0 * tol1; + + DBG(("linmin: Got bracket a:%f:%f x:%f:%f b:%f:%f\n",ax,af,xx,xf,bx,bf)) + + /* See if we're done */ +//printf("~1 linmin check %f <= %f\n",fabs(xx - mx), tol2 - 0.5 * (bx - ax)); + if (fabs(xx - mx) <= (tol2 - 0.5 * (bx - ax))) { + DBG(("linmin: We're done because %f <= %f\n",fabs(xx - mx), tol2 - 0.5 * (bx - ax))) + break; + } + + if (fabs(e) > tol1) { /* Do a trial parabolic fit */ + double te, p, q, r; + r = (xx-wx) * (xf-vf); + q = (xx-vx) * (xf-wf); + p = (xx-vx) * q - (xx-wx) * r; + q = 2.0 * (q - r); + if (q > 0.0) + p = -p; + else + q = -q; + te = e; /* Save previous e value */ + e = de; /* Previous steps distance moved */ + + DBG(("linmin: Trial parabolic fit\n" )) + + if (fabs(p) >= fabs(0.5 * q * te) || p <= q * (ax-xx) || p >= q * (bx-xx)) { + /* Give up on the parabolic fit, and use the golden section search */ + e = ((xx >= mx) ? ax-xx : bx-xx); /* Override previous distance moved */ + de = POWELL_CGOLD * e; + DBG(("linmin: Moving to golden section search\n" )) + } else { /* Use parabolic fit */ + de = p/q; /* Change in xb */ + ux = xx + de; /* Trial point according to parabolic fit */ + if ((ux - ax) < tol2 || (bx - ux) < tol2) { + if ((mx - xx) > 0.0) /* Don't use parabolic, use tol1 */ + de = tol1; /* tol1 is +ve */ + else + de = -tol1; + } + DBG(("linmin: Using parabolic fit\n" )) + } + } else { /* Keep using the golden section search */ + e = ((xx >= mx) ? ax-xx : bx-xx); /* Override previous distance moved */ + de = POWELL_CGOLD * e; + DBG(("linmin: Continuing golden section search\n" )) + } + + if (fabs(de) >= tol1) { /* If de moves as much as tol1 would */ + ux = xx + de; /* use it */ + DBG(("linmin: ux = %f = xx %f + de %f\n",ux,xx,de)) + } else { /* else move by tol1 in direction de */ + if (de > 0.0) { + ux = xx + tol1; + DBG(("linmin: ux = %f = xx %f + tol1 %f\n",ux,xx,tol1)) + } else { + ux = xx - tol1; + DBG(("linmin: ux = %f = xx %f - tol1 %f\n",ux,xx,tol1)) + } + } + + /* Evaluate function */ + for (i = 0; i < di; i++) + xt[i] = cp[i] + ux * xi[i]; + uf = (*func)(fdata, xt); + + if (uf <= xf) { /* Found new best solution */ + if (ux >= xx) { + ax = xx; af = xf; /* New lower bracket */ + } else { + bx = xx; bf = xf; /* New upper bracket */ + } + vx = wx; vf = wf; /* New previous 2nd best solution */ + wx = xx; wf = xf; /* New 2nd best solution from previous best */ + xx = ux; xf = uf; /* New best solution from latest */ + DBG(("linmin: found new best solution\n")) + } else { /* Found a worse solution */ + if (ux < xx) { + ax = ux; af = uf; /* New lower bracket */ + } else { + bx = ux; bf = uf; /* New upper bracket */ + } + if (uf <= wf || wx == xx) { /* New 2nd best solution, or equal best */ + vx = wx; vf = wf; /* New previous 2nd best solution */ + wx = ux; wf = uf; /* New 2nd best from latest */ + } else if (uf <= vf || vx == xx || vx == wx) { /* New 3rd best, or equal 1st & 2nd */ + vx = ux; vf = uf; /* New previous 2nd best from latest */ + } + DBG(("linmin: found new worse solution\n")) + } + } + /* !!! should do something if iter > POWELL_MAXIT !!!! */ + /* Solution is at xx, xf */ + + /* Compute solution vector */ + for (i = 0; i < di; i++) + cp[i] += xx * xi[i]; + } + + if (xt != XT) + free_dvector(xt,0,di-1); +//printf("~~~ line minimizer returning %e\n",xf); + return xf; +} + +#undef POWELL_GOLD +#undef POWELL_CGOLD +#undef POWELL_MAXIT + +/**************************************************/ diff --git a/numlib/powell.h b/numlib/powell.h new file mode 100644 index 0000000..a1db8b6 --- /dev/null +++ b/numlib/powell.h @@ -0,0 +1,74 @@ +#ifndef POWELL_H +#define POWELL_H + +/* Powell and Conjugate Gradient multivariate minimiser */ + +/* + * Copyright 2000 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +#ifdef __cplusplus + extern "C" { +#endif + +/* Standard interface for powell function */ +/* return 0 on sucess, 1 on failure due to excessive itterations */ +/* Result will be in cp */ +/* Arrays start at 0 */ +int powell( +double *rv, /* If not NULL, return the residual error */ +int di, /* Dimentionality */ +double cp[], /* Initial starting point */ +double s[], /* Size of initial search area */ +double ftol, /* Tollerance of error change to stop on */ +int maxit, /* Maximum iterations allowed */ +double (*funk)(void *fdata, double tp[]), /* Error function to evaluate */ +void *fdata, /* Opaque data needed by func() */ +void (*prog)(void *pdata, int perc), /* Optional progress percentage callback */ +void *pdata /* Opaque data needed by prog() */ +); + +/* Conjugate Gradient optimiser */ +/* return 0 on sucess, 1 on failure due to excessive itterations */ +/* Result will be in cp */ +int conjgrad( +double *rv, /* If not NULL, return the residual error */ +int di, /* Dimentionality */ +double cp[], /* Initial starting point */ +double s[], /* Size of initial search area */ +double ftol, /* Tollerance of error change to stop on */ +int maxit, /* Maximum iterations allowed */ +double (*func)(void *fdata, double tp[]), /* Error function to evaluate */ +double (*dfunc)(void *fdata, double dp[], double tp[]), /* Gradient function to evaluate */ +void *fdata, /* Opaque data needed by function */ +void (*prog)(void *pdata, int perc), /* Optional progress percentage callback */ +void *pdata /* Opaque data needed by prog() */ +); + +/* Example user function declarations */ +double powell_funk( /* Return function value */ + void *fdata, /* Opaque data pointer */ + double tp[]); /* Multivriate input value */ + +/* Line in multi-dimensional space minimiser */ +double brentnd( /* vector multiplier return value */ +double ax, /* Minimum of multiplier range */ +double bx, /* Starting point multiplier of search */ +double cx, /* Maximum of multiplier range */ +double ftol, /* Tollerance to stop search */ +double *xmin, /* Return value of multiplier at minimum */ +int n, /* Dimensionality */ +double (*func)(void *fdata, double tp[]), /* Error function to evaluate */ +void *fdata, /* Opaque data */ +double pcom[], /* Base vector point */ +double xicom[]); /* Vector that will be multiplied and added to pcom[] */ + +#ifdef __cplusplus + } +#endif + +#endif /* POWELL_H */ diff --git a/numlib/rand.c b/numlib/rand.c new file mode 100644 index 0000000..436de52 --- /dev/null +++ b/numlib/rand.c @@ -0,0 +1,108 @@ +/* Integer and floating point random number generator routines */ +/* + * Copyright 2000 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +#include "numsup.h" +#include "rand.h" + +/* 32 bit pseudo random sequencer based on XOR feedback */ +/* generates number between 1 and 4294967295 */ +#define PSRAND32(S) (((S) & 0x80000000) ? (((S) << 1) ^ 0xa398655d) : ((S) << 1)) + +/* 32 bit linear congruent generator */ +/* generates number between 0 and 4294967295 */ +/* (From Knuth & H.W.Lewis) */ +#define PSRAND32L(S) ((S) * 1664525L + 1013904223L) + +/* Return a 32 bit number between 0 and 4294967295 */ +/* Use Knuth shuffle to improve PSRAND32 sequence */ +unsigned int +rand32( /* Return 32 bit random number */ +unsigned int seed /* Optional seed. Non-zero re-initialized with that seed */ +) { +#define TSIZE 2843 /* Prime */ + static unsigned int last, ran = 0x12345678; /* Default seed */ + static unsigned int pvs[TSIZE]; + static int pvs_inited = 0; + int i; + + if (seed != 0) + { + pvs_inited = 0; + ran = seed;; + } + + /* Init random storage locations */ + if (pvs_inited == 0) + { + for (i = 0; i < TSIZE; i++) + pvs[i] = ran = PSRAND32(ran); + last = ran; + pvs_inited = 1; + } + i = last % TSIZE; /* New location */ + last = pvs[i]; /* Value generated */ + pvs[i] = ran = PSRAND32(ran); /* New value */ + + return last-1; +} + +/* return a random number between 0.0 and 1.0 */ +/* based on rand32 */ +double ranno(void) { + return rand32(0) / 4294967295.0; +} + +/* Return a random double in the range min to max */ +double +d_rand(double min, double max) { + double tt; + tt = ranno(); + return min + (max - min) * tt; +} + +/* Return a random integer in the range min to max inclusive */ +int +i_rand(int min, int max) { + double tt; + tt = ranno(); + return min + (int)floor(0.5 + ((double)(max - min)) * tt); +} + + +/* Return a random floating point number with a gausian/normal */ +/* distribution, centered about 0.0, with standard deviation 1.0 */ +/* This uses the Box-Muller transformation */ +double norm_rand(void) { + static int r2 = 0; /* Can use 2nd number generated */ + static double nr2; + + if (r2 == 0) { + double v1, v2, t1, t2, r1; + do { + v1 = d_rand(-1.0, 1.0); + v2 = d_rand(-1.0, 1.0); + t1 = v1 * v1 + v2 * v2; + } while (t1 == 0.0 || t1 >= 1.0); + t2 = sqrt(-2.0 * log(t1)/t1); + nr2 = v2 * t2; /* One for next time */ + r2 = 1; + r1 = v1 * t2; + return r1; + } else { + r2 = 0; + return nr2; + } +} + + + + + + + diff --git a/numlib/rand.h b/numlib/rand.h new file mode 100644 index 0000000..46f79f2 --- /dev/null +++ b/numlib/rand.h @@ -0,0 +1,36 @@ +#ifndef RAND_H +#define RAND_H + +/* + * Copyright 1998 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +#ifdef __cplusplus + extern "C" { +#endif + +/* Return a random number between 0 and 4294967294 */ +unsigned int +rand32( /* Return 32 bit random number */ +unsigned int seed); /* Optional seed. Non-zero re-initialized with that seed */ + +/* Return a random integer in the range min to max inclusive */ +int i_rand(int min, int max); + +/* Return a random double in the range min to max inclusive */ +double d_rand(double min, double max); + +/* Return a random floating point number with a gausian/normal */ +/* distribution, centered about 0.0, with standard deviation 1.0 */ +/* and an average deviation of 0.564 */ +double norm_rand(void); + +#ifdef __cplusplus + } +#endif + +#endif /* RAND_H */ diff --git a/numlib/sobol.c b/numlib/sobol.c new file mode 100644 index 0000000..40a2c38 --- /dev/null +++ b/numlib/sobol.c @@ -0,0 +1,211 @@ + +/***************************************************/ +/* Sobol sub-random vector sequence generator */ +/***************************************************/ + +/* Code is an expression of the algorithm decsribed in */ +/* the SSOBOL.F fortran source file, with additional */ +/* guidance from "Numerical Recipes in C", by W.H.Press, B.P.Flannery, */ +/* S.A.Teukolsky & W.T.Vetterling. */ + +/* + * Copyright 2002 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +#include "numsup.h" +#include "sobol.h" + +/* + * The array poly gives successive primitive + * polynomials coded in binary, e.g. + 45 = 100101 + * has bits 5, 2, and 0 set (counting from the + * right) and therefore represents + X**5 + X**2 + X**0 + + * These polynomials are in the order used by + * sobol in ussr comput. maths. math. phys. 16 (1977), + * 236-242. + */ + +static int sobol_poly[SOBOL_MAXDIM] = { + 1, 3, 7, 11, 13, 19, 25, 37, 59, 47, + 61, 55, 41, 67, 97, 91, 109, 103, 115, 131, + 193, 137, 145, 143, 241, 157, 185, 167, 229, 171, + 213, 191, 253, 203, 211, 239, 247, 285, 369, 299 +}; + +/* + * The initialization of the array vinit is from + * Sobol and Levitan, the production of points uniformly + * distributed in a multidimensional cube (in Russian), + * preprint ipm akad. nauk sssr, no. 40, moscow 1976. + * For a polynomial of degree m, m initial + * values are needed : these are the values given here. + * subsequent values are calculated during initialisation. + */ + +static int vinit[8][SOBOL_MAXDIM] = { + { + 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 + }, + { + 0, 0, 1, 3, 1, 3, 1, 3, 3, 1, + 3, 1, 3, 1, 3, 1, 1, 3, 1, 3, + 1, 3, 1, 3, 3, 1, 3, 1, 3, 1, + 3, 1, 1, 3, 1, 3, 1, 3, 1, 3 + }, + { + 0, 0, 0, 7, 5, 1, 3, 3, 7, 5, + 5, 7, 7, 1, 3, 3, 7, 5, 1, 1, + 5, 3, 3, 1, 7, 5, 1, 3, 3, 7, + 5, 1, 1, 5, 7, 7, 5, 1, 3, 3 + }, + { + 0, 0, 0, 0, 0, 1, 7, 9, 13, 11, + 1, 3, 7, 9, 5, 13, 13, 11, 3, 15, + 5, 3, 15, 7, 9, 13, 9, 1, 11, 7, + 5, 15, 1, 15, 11, 5, 3, 1, 7, 9 + }, + { + 0, 0, 0, 0, 0, 0, 0, 9, 3, 27, + 15, 29, 21, 23, 19, 11, 25, 7, 13, 17, + 1, 25, 29, 3, 31, 11, 5, 23, 27, 19, + 21, 5, 1, 17, 13, 7, 15, 9, 31, 9 + }, + { + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 37, 33, 7, 5, 11, 39, 63, + 27, 17, 15, 23, 29, 3, 21, 13, 31, 25, + 9, 49, 33, 19, 29, 11, 19, 27, 15, 25 + }, + { + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 13, + 33, 115, 41, 79, 17, 29, 119, 75, 73, 105, + 7, 59, 65, 21, 3, 113, 61, 89, 45, 107 + }, + { + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 7, 23, 39 + } +}; + +/* Get the next sobol vector */ +/* return nz if we've run out */ +static int next_sobol(sobol *s, double * v) +{ + int i, p; + unsigned int c; + + s->count++; + + /* Find the position of the right-hand zero in count */ + for (c = s->count, p = 0; (c & 1) == 0; p++, c >>= 1) + ; + + if(p > SOBOL_MAXBIT) + return 1; /* Run out */ + + for (i = 0; i < s->dim; i++) { + s->lastq[i] ^= s->dir[p][i]; + v[i] = s->lastq[i] * s->recipd; + } + + return 0; +} + +/* Free up the object */ +static void del_sobol(sobol *s) { + if (s != NULL) + free(s); +} + +/* reset the count */ +static void reset_sobol(sobol *s) { + int i; + + /* Set up first vector and values */ + s->count = 0; + for (i = 0; i < s->dim; i++) + s->lastq[i] = 0; +} + +/* Return NULL on error */ +sobol *new_sobol(int dim) { + sobol *s = NULL; + int i, j, p; + + if (dim < 1 || dim > SOBOL_MAXDIM) { + return NULL; + } + + if ((s = (sobol *)malloc(sizeof(sobol))) == NULL) { + return NULL; + } + + s->dim = dim; + s->next = next_sobol; + s->reset = reset_sobol; + s->del = del_sobol; + + /* Initialize the direction table */ + for (i = 0; i < dim; i++) { + + if (i == 0) { + for (j = 0; j < SOBOL_MAXBIT; j++) + s->dir[j][i] = 1; + } else { + int m; /* Degree */ + int pm; /* Polinomial mask */ + + /* Find degree of polynomial from binary encoding */ + for (m = 0, pm = sobol_poly[i] >> 1; pm != 0; m++, pm >>= 1) + ; + + /* The leading elements of row i come from vinit[][] */ + for (j = 0; j < m; j++) { + s->dir[j][i] = vinit[j][i]; + } + + /* Calculate remaining elements of row i as explained */ + /* in bratley and fox, section 2 */ + pm = sobol_poly[i]; + for (j = m; j < SOBOL_MAXBIT; j++) { + int k; + int newv = s->dir[j-m][i]; + for (k = 0; k < m; k++) { + if (pm & (1 << (m-k-1))) { + newv ^= s->dir[j-k-1][i] << (k+1); + } + } + s->dir[j][i] = newv; + } + } + } + /* Multiply columns of v by appropriate power of 2 */ + for (p = 2, j = SOBOL_MAXBIT-2; j >= 0; j--, p <<= 1) { + for (i = 0; i < dim; i++) + s->dir[j][i] *= p; + } + + /* recipd is 1/(common denominator of the elements in v) */ + s->recipd = 1.0/(1 << SOBOL_MAXBIT); + + /* Set up first vector and values */ + s->count = 0; + for (i = 0; i < dim; i++) + s->lastq[i] = 0; + + return s; +} + diff --git a/numlib/sobol.h b/numlib/sobol.h new file mode 100644 index 0000000..08ac5c0 --- /dev/null +++ b/numlib/sobol.h @@ -0,0 +1,52 @@ +#ifndef SOBOL_H +#define SOBOL_H + +#ifdef __cplusplus + extern "C" { +#endif + +#define SOBOL_MAXBIT 30 +#define SOBOL_MAXDIM 40 + +/* Object definition */ +struct _sobol { + /* Private: */ + int dim; /* dimension we're set for */ + unsigned int count; + double recipd; + int lastq[SOBOL_MAXDIM]; + int dir[SOBOL_MAXBIT][SOBOL_MAXDIM]; + + /* Public: */ + /* Methods */ + + /* Get the next sobol vector, return nz if we've run out */ + /* Values are between 0.0 and 1.0 */ + int (*next)(struct _sobol *s, double *v); + + /* Rest to the begining of the sequence */ + void (*reset)(struct _sobol *s); + + /* We're done with the object */ + void (*del)(struct _sobol *s); + +}; typedef struct _sobol sobol; + +/* Return NULL on error */ +sobol *new_sobol(int dim); + +#ifdef __cplusplus + } +#endif + +#endif /* SOBOL_H */ + + + + + + + + + + diff --git a/numlib/soboltest.c b/numlib/soboltest.c new file mode 100644 index 0000000..32c26ee --- /dev/null +++ b/numlib/soboltest.c @@ -0,0 +1,127 @@ + +/* Test the sobol sub-random vector sequencer */ + +#include <stdlib.h> +#include "numlib.h" + +/* Reference vectors */ +/* First 10 of dimension 40 */ +static double refv[10][40] = { + { + 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, + 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, + 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, + 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000, 0.5000 + }, { + 0.7500, 0.2500, 0.7500, 0.2500, 0.7500, 0.2500, 0.7500, 0.2500, 0.2500, 0.7500, + 0.2500, 0.7500, 0.2500, 0.7500, 0.2500, 0.7500, 0.7500, 0.2500, 0.7500, 0.2500, + 0.7500, 0.2500, 0.7500, 0.2500, 0.2500, 0.7500, 0.2500, 0.7500, 0.2500, 0.7500, + 0.2500, 0.7500, 0.7500, 0.2500, 0.7500, 0.2500, 0.7500, 0.2500, 0.7500, 0.2500 + }, { + 0.2500, 0.7500, 0.2500, 0.7500, 0.2500, 0.7500, 0.2500, 0.7500, 0.7500, 0.2500, + 0.7500, 0.2500, 0.7500, 0.2500, 0.7500, 0.2500, 0.2500, 0.7500, 0.2500, 0.7500, + 0.2500, 0.7500, 0.2500, 0.7500, 0.7500, 0.2500, 0.7500, 0.2500, 0.7500, 0.2500, + 0.7500, 0.2500, 0.2500, 0.7500, 0.2500, 0.7500, 0.2500, 0.7500, 0.2500, 0.7500 + }, { + 0.3750, 0.3750, 0.6250, 0.1250, 0.8750, 0.8750, 0.1250, 0.6250, 0.1250, 0.8750, + 0.3750, 0.6250, 0.1250, 0.3750, 0.6250, 0.1250, 0.6250, 0.3750, 0.3750, 0.8750, + 0.8750, 0.6250, 0.1250, 0.8750, 0.1250, 0.8750, 0.8750, 0.1250, 0.6250, 0.6250, + 0.3750, 0.3750, 0.3750, 0.3750, 0.6250, 0.1250, 0.8750, 0.8750, 0.1250, 0.6250 + }, { + 0.8750, 0.8750, 0.1250, 0.6250, 0.3750, 0.3750, 0.6250, 0.1250, 0.6250, 0.3750, + 0.8750, 0.1250, 0.6250, 0.8750, 0.1250, 0.6250, 0.1250, 0.8750, 0.8750, 0.3750, + 0.3750, 0.1250, 0.6250, 0.3750, 0.6250, 0.3750, 0.3750, 0.6250, 0.1250, 0.1250, + 0.8750, 0.8750, 0.8750, 0.8750, 0.1250, 0.6250, 0.3750, 0.3750, 0.6250, 0.1250 + }, { + 0.6250, 0.1250, 0.3750, 0.3750, 0.1250, 0.6250, 0.8750, 0.8750, 0.3750, 0.1250, + 0.1250, 0.3750, 0.3750, 0.6250, 0.8750, 0.8750, 0.3750, 0.1250, 0.6250, 0.6250, + 0.1250, 0.8750, 0.8750, 0.6250, 0.3750, 0.1250, 0.6250, 0.8750, 0.8750, 0.3750, + 0.1250, 0.6250, 0.6250, 0.1250, 0.3750, 0.3750, 0.1250, 0.6250, 0.8750, 0.8750 + }, { + 0.1250, 0.6250, 0.8750, 0.8750, 0.6250, 0.1250, 0.3750, 0.3750, 0.8750, 0.6250, + 0.6250, 0.8750, 0.8750, 0.1250, 0.3750, 0.3750, 0.8750, 0.6250, 0.1250, 0.1250, + 0.6250, 0.3750, 0.3750, 0.1250, 0.8750, 0.6250, 0.1250, 0.3750, 0.3750, 0.8750, + 0.6250, 0.1250, 0.1250, 0.6250, 0.8750, 0.8750, 0.6250, 0.1250, 0.3750, 0.3750 + }, { + 0.1875, 0.3125, 0.3125, 0.6875, 0.5625, 0.1875, 0.0625, 0.9375, 0.1875, 0.0625, + 0.6875, 0.8125, 0.5625, 0.6875, 0.1875, 0.6875, 0.1875, 0.0625, 0.0625, 0.8125, + 0.9375, 0.3125, 0.5625, 0.3125, 0.4375, 0.4375, 0.6875, 0.4375, 0.8125, 0.5625, + 0.9375, 0.8125, 0.1875, 0.3125, 0.3125, 0.6875, 0.5625, 0.1875, 0.0625, 0.9375 + }, { + 0.6875, 0.8125, 0.8125, 0.1875, 0.0625, 0.6875, 0.5625, 0.4375, 0.6875, 0.5625, + 0.1875, 0.3125, 0.0625, 0.1875, 0.6875, 0.1875, 0.6875, 0.5625, 0.5625, 0.3125, + 0.4375, 0.8125, 0.0625, 0.8125, 0.9375, 0.9375, 0.1875, 0.9375, 0.3125, 0.0625, + 0.4375, 0.3125, 0.6875, 0.8125, 0.8125, 0.1875, 0.0625, 0.6875, 0.5625, 0.4375 + }, { + 0.9375, 0.0625, 0.5625, 0.9375, 0.3125, 0.4375, 0.8125, 0.6875, 0.4375, 0.8125, + 0.9375, 0.0625, 0.8125, 0.4375, 0.4375, 0.4375, 0.9375, 0.3125, 0.8125, 0.5625, + 0.1875, 0.0625, 0.3125, 0.0625, 0.1875, 0.6875, 0.9375, 0.6875, 0.5625, 0.3125, + 0.6875, 0.0625, 0.9375, 0.0625, 0.5625, 0.9375, 0.3125, 0.4375, 0.8125, 0.6875 + } +}; + +int main() { + sobol *s; + int i; + double vec[40]; + int fail = 0; + + printf("Starting sobol test\n"); + + if ((s = new_sobol(40)) == NULL) { + printf("new_sobol failed\n"); + exit(1); + } + + for (i = 0; i < 10; i++) { + int j; + if (s->next(s, vec)) { + printf("Next failed\n"); + exit(1); + } + printf("Vector %d = ",i); + for (j = 0; j < 40; j++) { + printf("%f ",vec[j]); + } + printf("\n"); + + for (j = 0; j < 40; j++) { + if (refv[i][j] != vec[j]) { + printf("Warning: failed at vector %d, dim %d (%f != %f)\n",i,j,refv[i][j],vec[j]); + fail = 1; + } + } + } + + s->del(s); + if (fail) { + printf("Sobol test FAILED\n"); + return 1; + } else { + printf("Sobol test done OK\n"); + return 0; + } +} + + + + + + + + + + + + + + + + + + + + + + + diff --git a/numlib/svd.c b/numlib/svd.c new file mode 100644 index 0000000..fdb8edd --- /dev/null +++ b/numlib/svd.c @@ -0,0 +1,611 @@ + +/* + * Singular Value Decomposition, + * from the Burton S. Garbow's EISPACK FORTRAN code, + * based on the algorithm of Golub and Reinsch in + * "Handbook of Automatic Computation", + * with some guidance from R. B Davie's newmat09. + * + * Copyright 2000 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +#include "numsup.h" +#include "svd.h" + +#ifndef NEVER +/* Compute the pythagorian distance sqrt(a^2 + b^2) */ +/* taking care of under or overflow. */ +double pythag( +double a, +double b) { + double aba, abb; + + aba = fabs(a); + abb = fabs(b); + + if (aba > abb) { + double boa; + boa = abb/aba; + return aba * sqrt(1.0 + boa * boa); + } else { + double aob; + if (abb == 0.0) + return 0.0; + aob = aba/abb; + return abb * sqrt(1.0 + aob * aob); + } +} +#else /* Quicker, but less robust */ +#define pythag(a, b) sqrt((a) * (a) + (b) * (b)) +#endif + +#define MAXITS 30 + +/* Compute Singular Value Decomposition of A = U.W.Vt */ +/* Return status value: */ +/* 0 - no error */ +/* 1 - Too many itterations */ +int svdecomp( +double **a, /* A[0..m-1][0..n-1], return U[][] */ +double *w, /* return W[0..n-1] */ +double **v, /* return V[0..n-1][0..n-1] (not transpose!) */ +int m, /* Number of equations */ +int n /* Number of unknowns */ +) { + double eps = DBL_EPSILON; /* 1.0 + eps = 1.0 */ + double tol = DBL_MIN/eps; /* Minumum +ve value/eps */ + double *rv1, RV1[10]; + double anm; + int i, j, k; + int its; + + if (n <= 10) + rv1 = RV1; /* Allocate fast off stack */ + else + rv1 = dvector(0, n-1); + + /* Housholder reduction of A to bidiagonal form */ + anm = 0.0; + rv1[0] = 0.0; /* Will always == 0.0 */ + for (i = 0; i < n; i++) { /* For each element in the diagonal of A */ + int ip1 = i + 1; + + /* Deal with lower column at i */ + w[i] = 0.0; + if (i < m) { /* If it makes sense to go from row i .. m-1 */ + double ss, ff = 0.0; + + for (ss = 0.0, k = m-1; k >= i; k--) { /* Sum of squares of column */ + ff = a[k][i]; + ss += ff * ff; + } /* Note ff = A[i][i] */ + if (ss >= tol) { + double gg, hh; + gg = sqrt(ss); + w[i] = gg = ff < 0.0 ? gg : -gg; /* gg has -sign of ff */ + hh = ff * gg - ss; + a[i][i] = ff - gg; + + /* For all lower columns to the right of this one */ + for (j = ip1; j < n; j++) { /* Column j */ + double tt; + for (ss = 0.0, k = i; k < m; k++) + ss += a[k][j] * a[k][i]; /* Sum of products of i and j columns */ + tt = ss / hh; + for (k = i; k < m; k++) + a[k][j] += tt * a[k][i]; /* Add sumprod/hh to column j */ + } + } + } + + /* Deal with upper super row at i */ + if (ip1 < n) { /* If it makes sense to go from column i+1 .. n-1 */ + rv1[ip1] = 0.0; + if (i < m) { /* If it makes sense to process row i */ + double ss, ff = 0.0; + for (ss = 0.0, k = n-1; k >= ip1; k--) { /* Sum of squares of row */ + ff = a[i][k]; + ss += ff * ff; + } /* Note ff = A[i][ip1] */ + if (ss >= tol) { + double gg, hh; + gg = sqrt(ss); + rv1[ip1] = gg = ff < 0.0 ? gg : -gg; /* gg has -sign of ff */ + hh = ff * gg - ss; + a[i][ip1] = (ff - gg); + + /* For all upper rows below this one */ + for (j = ip1; j < m; j++) { + double tt; + for (ss = 0.0, k = ip1; k < n; k++) /* Sum of products of i and j rows */ + ss += a[j][k] * a[i][k]; + tt = ss / hh; + for (k = ip1; k < n; k++) + a[j][k] += tt * a[i][k]; /* Add sumprod/hh to row j */ + } + } + } + } + { + double tt; + tt = fabs(w[i]) + fabs(rv1[i]); + if (tt > anm) + anm = tt; + } + } + + /* Accumulation of right hand transformations */ + for (i = n-1; i >= 0; i--) { + int ip1 = i + 1; + if (ip1 < n) { + double gg; + gg = rv1[ip1]; + if (gg != 0.0) { + gg = 1.0 / gg; + for (j = ip1; j < n; j++) + v[j][i] = (a[i][j] / a[i][ip1]) * gg; /* Double division to avoid underflow */ + for (j = ip1; j < n; j++) { + double ss; + for (ss = 0.0, k = ip1; k < n; k++) + ss += a[i][k] * v[k][j]; + for (k = ip1; k < n; k++) + v[k][j] += ss * v[k][i]; + } + } + for (j = ip1; j < n; j++) + v[i][j] = v[j][i] = 0.0; + } + v[i][i] = 1.0; + } + /* Accumulation of left hand transformations */ + for (i = n < m ? n-1 : m-1; i >= 0; i--) { + int ip1 = i + 1; + double gg = w[i]; + if (ip1 < n) + for (j = ip1; j < n; j++) + a[i][j] = 0.0; + if (gg == 0.0) { + for (j = i; j < m; j++) + a[j][i] = 0.0; + } else { + gg = 1.0 / gg; + if (ip1 < n) { + for (j = ip1; j < n; j++) { + double ss, ff; + for (ss = 0.0, k = ip1; k < m; k++) + ss += a[k][i] * a[k][j]; + ff = (ss / a[i][i]) * gg; /* Double division to avoid underflow */ + for (k = i; k < m; k++) + a[k][j] += ff * a[k][i]; + } + } + for (j = i; j < m; j++) + a[j][i] *= gg; + } + a[i][i] += 1.0; + } + + eps *= anm; + + /* Fully diagonalize bidiagonal result, by */ + /* successive QR rotations. */ + for (k = (n-1); k >= 0; k--) { /* For all the singular values */ + for (its = 0;; its++) { + int flag; + int lm1 = 0; + int ll; + double zz; + + /* Test for splitting */ + for (flag = 1, ll = k; ll >= 0; ll--) { + lm1 = ll - 1; + if (fabs(rv1[ll]) <= eps) { /* Note always stops at 0 because rv1[0] = 0.0 */ + flag = 0; + break; + } + if (fabs(w[lm1]) <= eps) + break; + } + if (flag != 0) { + double cc = 0.0; + double ss = 1.0; + for (i = ll; i <= k; i++) { + double ff, gg, hh; + gg = rv1[i]; + rv1[i] = cc * gg; + ff = ss * gg; + if (fabs(ff) <= eps) + break; /* Found acceptable solution */ + gg = w[i]; + w[i] = hh = pythag(ff, gg); + hh = 1.0 / hh; + cc = gg * hh; + ss = -ff * hh; + + /* Apply rotation */ + for (j = 0; j < m; j++) { + double y1, z1; + y1 = a[j][lm1]; + z1 = a[j][i]; + a[j][lm1] = y1 * cc + z1 * ss; + a[j][i] = z1 * cc - y1 * ss; + } + } + } + zz = w[k]; + if (k == ll) { /* Convergence */ + if (zz < 0.0) { + w[k] = -zz; /* Make singular value non-negative */ + for (j = 0; j < n; j++) + v[j][k] = (-v[j][k]); + } + break; + } + if (its == MAXITS) { +/* fprintf(stderr,"No convergence in %d SVDCMP iterations",MAXITS); */ + if (rv1 != RV1) + free_dvector(rv1, 0, n-1); + return 1; + } + { + double ff, gg, hh, cc, ss, xx, yy; + int km1; + + km1 = k - 1; + xx = w[ll]; + yy = w[km1]; + gg = rv1[km1]; + hh = rv1[k]; + ff = ((yy - zz) * (yy + zz) + (gg - hh) * (gg + hh)) / (2.0 * hh * yy); + gg = pythag(ff, 1.0); + gg = ff < 0.0 ? -gg : gg; + ff = ((xx - zz) * (xx + zz) + hh * ((yy / (ff + gg)) - hh)) / xx; + cc = ss = 1.0; + + for (j = ll; j <= km1; j++) { + double f2, g2, y2, h2, z2; + int jp1 = j + 1; + g2 = rv1[jp1]; + y2 = w[jp1]; + h2 = ss * g2; + g2 = cc * g2; + rv1[j] = z2 = pythag(ff, h2); + cc = ff / z2; + ss = h2 / z2; + f2 = xx * cc + g2 * ss; + g2 = g2 * cc - xx * ss; + h2 = y2 * ss; + y2 = y2 * cc; + + /* Apply rotation */ + for (i = 0; i < n; i++) { + double x1, z1; + x1 = v[i][j]; + z1 = v[i][jp1]; + v[i][j] = x1 * cc + z1 * ss; + v[i][jp1] = z1 * cc - x1 * ss; + } + w[j] = z2 = pythag(f2, h2); + if (z2 != 0.0) { /* Rotation can be arbitrary */ + z2 = 1.0 / z2; + cc = f2 * z2; + ss = h2 * z2; + } + ff = (cc * g2) + (ss * y2); + xx = (cc * y2) - (ss * g2); + + /* Apply rotation */ + for (i = 0; i < m; i++) { + double y1, z1; + y1 = a[i][j]; + z1 = a[i][jp1]; + a[i][j] = y1 * cc + z1 * ss; + a[i][jp1] = z1 * cc - y1 * ss; + } + } + rv1[ll] = 0.0; + rv1[k] = ff; + w[k] = xx; + } + } + } + if (rv1 != RV1) + free_dvector(rv1, 0, n-1); + + return 0; +} + +/* --------------------------- */ +/* Threshold the singular values W[] */ +void svdthresh( +double w[], /* Singular values */ +int n /* Number of unknowns */ +) { + int i; + double maxw; + + /* Threshold the w[] values */ + for (maxw = 0.0, i = 0; i < n; i++) { + if (w[i] > maxw) + maxw = w[i]; + } + maxw *= 1.0e-12; + for (i = 0; i < n; i++) { + if (w[i] < maxw) + w[i] = 0.0; + } +} + +/* --------------------------- */ +/* Threshold the singular values W[] to give */ +/* a specific degree of freedom. */ +void svdsetthresh( +double w[], /* Singular values */ +int n, /* Number of unknowns */ +int dof /* Expected degree of freedom */ +) { + int i, j; + + /* Set the dof smallest elements to zero */ + /* (This algorithm is simple but not quick) */ + for (j = 0; j < dof;) { + int k; + double minv = 1e38; + for (k = j = i = 0; i < n; i++) { + if (w[i] == 0.0) { + j++; + continue; + } + if (w[i] < minv) { + minv = w[i]; + k = i; + } + } + if (j < dof) /* Zero next smallest */ + w[k] = 0.0; + } +} + +/* --------------------------- */ +/* Use output of svdcmp() to solve overspecified and/or */ +/* singular equation A.x = b */ +int svdbacksub( +double **u, /* U[0..m-1][0..n-1] U, W, V SVD decomposition of A[][] */ +double *w, /* W[0..n-1] */ +double **v, /* V[0..n-1][0..n-1] (not transpose!) */ +double b[], /* B[0..m-1] Right hand side of equation */ +double x[], /* X[0..n-1] Return solution. (May be the same as b[]) */ +int m, /* Number of equations */ +int n /* Number of unknowns */ +) { + int i, j; + double *tmp, TMP[10]; /* Intermediate value of B . U-1 . W-1 */ + + if (n <= 10) + tmp = TMP; + else + tmp = dvector(0, n-1); + + /* A . X = B == U . W . Vt . X = B */ + /* and U, W, and Vt are trivialy invertable */ + + /* Compute B . U-1 . W-1 */ + for (j = 0; j < n; j++) { + if (w[j]) { + double s; + for (s = 0.0, i = 0; i < m; i++) + s += b[i] * u[i][j]; + s /= w[j]; + tmp[j] = s; + } else { + tmp[j] = 0.0; + } + } + /* Compute T. V-1 */ + for (j = 0; j < n; j++) { + double s; + for (s = 0.0, i = 0; i < n; i++) + s += v[j][i] * tmp[i]; + x[j] = s; + } + if (tmp != TMP) + free_dvector(tmp, 0, n-1); + return 0; +} + + +/* --------------------------- */ +/* Solve the equation A.x = b using SVD */ +/* (The w[] values are thresholded for best accuracy) */ +/* Return non-zero if no solution found */ +int svdsolve( +double **a, /* A[0..m-1][0..n-1] input A[][], will return U[][] */ +double b[], /* B[0..m-1] Right hand side of equation, return solution */ +int m, /* Number of equations */ +int n /* Number of unknowns */ +) { + int i; + double *w, W[8]; + double **v, *VP[8], V[8][8]; + double maxw; + + if (n <= 8) { + w = W; + VP[0] = V[0]; VP[1] = V[1]; VP[2] = V[2]; VP[3] = V[3]; + VP[4] = V[4]; VP[5] = V[5]; VP[6] = V[6]; VP[7] = V[7]; + v = VP; + } else { + w = dvector(0, n-1); + v = dmatrix(0, n-1, 0, n-1); + } + + /* Singular value decompose */ + if (svdecomp(a, w, v, m, n)) { + if (w != W) { + free_dvector(w, 0, n-1); + free_dmatrix(v, 0, n-1, 0, n-1); + } + return 1; + } + + /* Threshold the w[] values */ + for (maxw = 0.0, i = 0; i < n; i++) { + if (w[i] > maxw) + maxw = w[i]; + } + maxw *= 1.0e-12; + for (i = 0; i < n; i++) { + if (w[i] < maxw) + w[i] = 0.0; + } + + /* Back substitute to solve the equation */ + svdbacksub(a, w, v, b, b, m, n); + + if (w != W) { + free_dvector(w, 0, n-1); + free_dmatrix(v, 0, n-1, 0, n-1); + } + return 0; +} + + +/* --------------------------- */ +/* Solve the equation A.x = b using SVD */ +/* The top s out of n singular values will be used */ +/* Return non-zero if no solution found */ +int svdsolve_s( +double **a, /* A[0..m-1][0..n-1] input A[][], will return U[][] */ +double b[], /* B[0..m-1] Right hand side of equation, return solution */ +int m, /* Number of equations */ +int n, /* Number of unknowns */ +int s /* Number of singular values */ +) { + int i, j; + double *w, W[8]; + int *sw, SW[8]; + double **v, *VP[8], V[8][8]; + double maxw; + + if (n <= 8) { + w = W; + sw = SW; + VP[0] = V[0]; VP[1] = V[1]; VP[2] = V[2]; VP[3] = V[3]; + VP[4] = V[4]; VP[5] = V[5]; VP[6] = V[6]; VP[7] = V[7]; + v = VP; + } else { + w = dvector(0, n-1); + sw = ivector(0, n-1); + v = dmatrix(0, n-1, 0, n-1); + } + + /* Singular value decompose */ + if (svdecomp(a, w, v, m, n)) { + if (w != W) { + free_dvector(w, 0, n-1); + free_dmatrix(v, 0, n-1, 0, n-1); + } + return 1; + } + + /* Create sorted index of w[] */ + for (maxw = 0.0, i = 0; i < n; i++) { + sw[i] = i; + if (w[i] > maxw) + maxw = w[i]; + } + maxw *= 1.0e-12; + + /* Really dumb exchange sort.. */ + for (i = 0; i < (n-1); i++) { + for (j = i+1; j < n; j++) { + if (w[sw[i]] > w[sw[j]]) { + int tt = sw[i]; + sw[i] = sw[j]; + sw[j] = tt; + } + } + } + + /* Set the (n - s) smallest values to zero */ + s = n - s; + if (s < 0) + s = 0; + if (s > n) + s = n; + for (i = 0; i < s; i++) + w[sw[i]] = 0.0; + + /* And threshold them too */ + for (maxw = 0.0, i = 0; i < n; i++) { + if (w[i] < maxw) + w[i] = 0.0; + } + + /* Back substitute to solve the equation */ + svdbacksub(a, w, v, b, b, m, n); + + if (w != W) { + free_dvector(w, 0, n-1); + free_ivector(sw, 0, n-1); + free_dmatrix(v, 0, n-1, 0, n-1); + } + return 0; +} + + +/* --------------------------- */ +/* Solve the equation A.x = b using Direct calculation, LU or SVD as appropriate */ +/* Return non-zero if no solution found */ + +#include "ludecomp.h" + +int gen_solve_se( +double **a, /* A[0..m-1][0..n-1] input A[][], will return U[][] */ +double b[], /* B[0..m-1] Right hand side of equation, return solution */ +int m, /* Number of equations */ +int n /* Number of unknowns */ +) { + if (n == m) { + if (n == 1) { /* So simple, solve it directly */ + double tt = a[0][0]; + if (fabs(tt) <= DBL_MIN) + return 1; + b[0] = b[0]/tt; + return 0; + } else { + return solve_se(a, b, n); + } + } else { + return svdsolve(a, b, m, n); + } +} + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/numlib/svd.h b/numlib/svd.h new file mode 100644 index 0000000..59b080d --- /dev/null +++ b/numlib/svd.h @@ -0,0 +1,90 @@ +#ifndef SVD_H +#define SVD_H + +/* + * Copyright 2000 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +#ifdef __cplusplus + extern "C" { +#endif + +/* Compute Singular Value Decomposition of A = U.W.Vt */ +/* Return status value: */ +/* 0 - no error */ +/* 1 - m < n error */ +int svdecomp( +double **a, /* A[0..m-1][0..n-1], return U[][] */ +double *w, /* return W[0..n-1] */ +double **v, /* return V[0..n-1][0..n-1] (not transpose!) */ +int m, /* Number of equations */ +int n /* Number of unknowns */ +); + +/* Threshold the singular values W[] */ +void svdthresh( +double w[], /* Singular values */ +int n /* Number of unknowns */ +); + +/* Threshold the singular values W[] to give a specific dof */ +void svdsetthresh( +double w[], /* Singular values */ +int n, /* Number of unknowns */ +int dof /* Expected degree of freedom */ +); + +/* Use output of svdcmp() to solve overspecified and/or */ +/* singular equation A.x = b */ +int svdbacksub( +double **u, /* U[0..m-1][0..n-1] U, W, V SVD decomposition of A[][] */ +double *w, /* W[0..n-1] */ +double **v, /* V[0..n-1][0..n-1] (not transpose!) */ +double b[], /* B[0..m-1] Right hand side of equation */ +double x[], /* X[0..n-1] Return solution. (May be the same as b[]) */ +int m, /* Number of equations */ +int n /* Number of unknowns */ +); + +/* Solve the equation A.x = b using SVD */ +/* (The w[] values are thresholded for best accuracy) */ +/* Return non-zero if no solution found */ +/* !!! Note that A[][] will be changed !!! */ +int svdsolve( +double **a, /* A[0..m-1][0..n-1] input A[][], will return U[][] */ +double b[], /* B[0..m-1] Right hand side of equation, return solution */ +int m, /* Number of equations */ +int n /* Number of unknowns */ +); + +/* Solve the equation A.x = b using SVD */ +/* The top s out of n singular values will be used */ +/* Return non-zero if no solution found */ +/* !!! Note that A[][] will be changed !!! */ +int svdsolve_s( +double **a, /* A[0..m-1][0..n-1] input A[][], will return U[][] */ +double b[], /* B[0..m-1] Right hand side of equation, return solution */ +int m, /* Number of equations */ +int n, /* Number of unknowns */ +int s /* Number of unknowns */ +); + +/* Solve the equation A.x = b using Direct calculation, LU or SVD as appropriate */ +/* Return non-zero if no solution found */ +/* !!! Note that A[][] will be changed !!! */ +int gen_solve_se( +double **a, /* A[0..m-1][0..n-1] input A[][], will return U[][] */ +double b[], /* B[0..m-1] Right hand side of equation, return solution */ +int m, /* Number of equations */ +int n /* Number of unknowns */ +); + +#ifdef __cplusplus + } +#endif + +#endif /* SVD_H */ diff --git a/numlib/svdtest.c b/numlib/svdtest.c new file mode 100644 index 0000000..100120d --- /dev/null +++ b/numlib/svdtest.c @@ -0,0 +1,214 @@ + +/* SVD test */ +/* Verify that the SVD solver does what it is supposed to. */ +/* + * Copyright 2000 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +/* We assume two device dependent variables, and one objective function value */ + +#include <stdio.h> +#include <stdlib.h> +#include <stdarg.h> +#include <math.h> +#include <time.h> +#if defined(__IBMC__) +#include <float.h> +#endif + +#include "numlib.h" + +int main(void) { + int its; + int i,j,x; + double **a; /* A[0..M-1][0..N-1] input */ + double **u; /* U[0..M-1][0..N-1] output */ + double *w; /* W[0..N-1] output */ + double **v; /* V[0..N-1][0..N-1] output */ + double **a2; /* A[0..M-1][0..N-1] check on a */ + double **t; /* A[0..M-1][0..N-1] temp */ + int m,n; + +#if defined(__IBMC__) + _control87(EM_UNDERFLOW, EM_UNDERFLOW); + _control87(EM_OVERFLOW, EM_OVERFLOW); +#endif + + printf("Test SVD\n"); + + for (its = 400; its > 0; its--) { + int bad; + int bad0; + + m = i_rand(1,30); /* Number of equations */ + n = i_rand(1,30); /* Number of unknowns */ + + a = dmatrix(0,m-1, 0,n-1); + t = dmatrix(0,m-1, 0,n-1); + a2 = dmatrix(0,m-1, 0,n-1); + u = dmatrix(0,m-1, 0,n-1); + w = dvector(0,n-1); + v = dmatrix(0,n-1, 0,n-1); + + printf("Testing %d by %d\n",m,n); + + /* Create A matrix */ + for (j = 0; j < m; j++) + for (i = 0; i < n; i++) + a[j][i] = d_rand(-10.0, 10.0); + + /* Setup u */ + for (j = 0; j < m; j++) + for (i = 0; i < n; i++) + u[j][i] = a[j][i]; + + /* decompose A into U, W and V */ + svdecomp(u, w, v, m, n); + + /* Check results by computing a2 = U.W.Vt */ + for (j = 0; j < m; j++) { /* U.W */ + for (i = 0; i < n; i++) { + t[j][i] = 0.0; + for (x = 0; x < n; x++) { + if (x == i) + t[j][i] += u[j][x] * w[x]; + } + } + } + for (j = 0; j < m; j++) { /* .Vt */ + for (i = 0; i < n; i++) { + a2[j][i] = 0.0; + for (x = 0; x < n; x++) { + a2[j][i] += t[j][x] * v[i][x]; + } + } + } + + /* Now check */ + bad = 0; + for (j = 0; j < m; j++) + for (i = 0; i < n; i++) { + double tt; + tt = a2[j][i] - a[j][i]; + tt = fabs(tt); + if (tt > 0.0000001) { + bad = 1; + } + } + if (bad) + printf("A == U.W.Vt Check failed!\n"); + + + /* Check that U and Ut are inverses */ + bad = bad0 = 0; + for (j = 0; j < n; j++) { + for (i = 0; i < n; i++) { + double t2, tt = 0.0; + for (x = 0; x < m; x++) { + tt += u[x][j] * u[x][i]; + } + t2 = tt; + if (i == j) + tt -= 1.0; + tt = fabs(tt); + if (tt > 0.0000001) { + if (i == j && fabs(t2) < 0.0000001) + bad0++; /* Unexpected zero diagonal */ + else { + bad = 1; + printf("Possible U error at %d %d = %f \n",j,i,tt); + } + } + } + } + /* Expect n-m diagnals to be 0 instead of 1 if m < n */ + if (bad || (m >= n && bad0) || (m < n && bad0 != n-m)) + printf("U,Ut == 1 Check failed!\n"); + + /* Check that V and Vt are inverses */ + bad = 0; + for (j = 0; j < n; j++) { + for (i = 0; i < n; i++) { + double tt = 0.0; + for (x = 0; x < n; x++) { + tt += v[j][x] * v[i][x]; + } + if (i == j) + tt -= 1.0; + tt = fabs(tt); + if (tt > 0.0000001) { + bad = 1; + printf("V Error at %d %d = %f \n",j,i,tt); + } + } + if (bad) + printf("V,Vt == 1 Check failed!\n"); + } + +#ifdef NEVER + printf("A = %f %f %f\n %f %f %f\n %f %f %f\n\n", + a[0][0], a[0][1], a[0][2], a[1][0], a[1][1], a[1][2], a[2][0], a[2][1], a[2][2]); + + printf("u = %f %f %f\n %f %f %f\n %f %f %f\n\n", + u[0][0], u[0][1], u[0][2], u[1][0], u[1][1], u[1][2], u[2][0], u[2][1], u[2][2]); + + printf("w = %f %f %f\n\n", w[0],w[1],w[2]); + + printf("V = %f %f %f\n %f %f %f\n %f %f %f\n\n", + v[0][0], v[0][1], v[0][2], v[1][0], v[1][1], v[1][2], v[2][0], v[2][1], v[2][2]);; + + printf("A2 = %f %f %f\n %f %f %f\n %f %f %f\n\n", + a2[0][0], a2[0][1], a2[0][2], a2[1][0], a2[1][1], + a2[1][2], a2[2][0], a2[2][1], a2[2][2]); +#endif + + free_dmatrix(a, 0,m-1, 0,n-1); + free_dmatrix(t, 0,m-1, 0,n-1); + free_dmatrix(a2, 0,m-1, 0,n-1); + free_dmatrix(u, 0,m-1, 0,n-1); + free_dvector(w, 0,n-1); + free_dmatrix(v, 0,n-1, 0,n-1); + } + return 0; +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/numlib/tdhsx.c b/numlib/tdhsx.c new file mode 100644 index 0000000..d1738c4 --- /dev/null +++ b/numlib/tdhsx.c @@ -0,0 +1,117 @@ + +/* + * Code to test the downhill simplex minimiser + * + * Copyright 1999 - 2005 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +#include <stdio.h> +#include "numlib.h" + +/* Final approximate solution: */ + +double expect[9] = { + -0.5706545E+00, + -0.6816283E+00, + -0.7017325E+00, + -0.7042129E+00, + -0.7013690E+00, + -0.6918656E+00, + -0.6657920E+00, + -0.5960342E+00, + -0.4164121E+00 }; + +double fcn( /* Return function value */ + void *fdata, /* Opaque data pointer */ + double tp[]); /* Multivriate input value */ + +#define N 9 + +int main(void) +{ + double cp[N]; /* Function input values */ + double s[N]; /* Search area */ + double err; + int j; + int nprint = 0; /* Itteration debugging print = off */ + int rv; + + error_program = "tdhsx"; /* Set global error reporting string */ + + /* The following starting values provide a rough solution. */ + for (j = 0; j < N; j++) { + cp[j] = -1.f; + s[j] = 0.9; + } + + nprint = 0; + + /* Set tol to the square root of the machine precision. */ + /* Unless high precision solutions are required, */ + /* this is the recommended setting. */ + + rv = dhsx( + &err, /* return residual */ + N, /* Dimentionality */ + cp, /* Initial starting point */ + s, /* Size of initial search area */ + 0.0000001, /* Tollerance of error change to stop on */ + 1e6, /* Absolute error to stop on */ +// 10.0, /* Tollerance of error change to stop on */ +// 0.0001, /* Absolute error to stop on */ + 1000, /* Maximum iterations allowed */ + fcn, /* Error function to evaluate */ + NULL); /* Opaque data needed by function */ + + + fprintf(stdout,"Return code %d, residual err = %f:\n",rv, err); + for (j = 0; j < N; j++) { + fprintf(stdout,"cp[%d] = %e, expect %e\n",j,cp[j],expect[j]); + } + + return 0; +} /* main() */ + +/* Function being minimized */ +double fcn( /* Return function value */ +void *fdata, /* Opaque data pointer */ +double tp[] /* Multivriate input value */ +) { + double err, tt; + double temp, temp1, temp2; + int k; + + /* Function Body */ + err = 0.0; + for (k = 0; k < N; ++k) { + temp = (3.0 - 2.0 * tp[k]) * tp[k]; + temp1 = 0.0; + if (k != 0) { + temp1 = tp[k-1]; + } + temp2 = 0.0; + if (k != ((N)-1)) + temp2 = tp[k+1]; + tt = temp - temp1 - 2.0 * temp2 + 1.0; + err += tt * tt; + } + err = sqrt(err); +// printf("Returning %16.14f\n",err); + return err; +} + + + + + + + + + + + + diff --git a/numlib/tpowell.c b/numlib/tpowell.c new file mode 100644 index 0000000..f7b0aad --- /dev/null +++ b/numlib/tpowell.c @@ -0,0 +1,123 @@ +/* Code to test the powell minimiser */ +/* + * Copyright 1999 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +#include <stdio.h> +#include "numlib.h" + +/* Final approximate solution: */ + +double expect[9] = { + -0.5706545E+00, + -0.6816283E+00, + -0.7017325E+00, + -0.7042129E+00, + -0.7013690E+00, + -0.6918656E+00, + -0.6657920E+00, + -0.5960342E+00, + -0.4164121E+00 }; + +double fcn( /* Return function value */ + void *fdata, /* Opaque data pointer */ + double tp[]); /* Multivriate input value */ + +#define N 9 + +static void progress(void *pdata, int perc) { + printf("%c% 3d%%",cr_char,perc); + if (perc == 100) + printf("\n"); + fflush(stdout); +} + +int main(void) +{ + double cp[N]; /* Function input values */ + double s[N]; /* Search area */ + double err; + int j; + int nprint = 0; /* Itteration debugging print = off */ + int rc; + + error_program = "tpowell"; /* Set global error reporting string */ + check_if_not_interactive(); + + /* The following starting values provide a rough solution. */ + for (j = 0; j < N; j++) { + cp[j] = -1.f; + s[j] = 0.9; + } + + nprint = 0; + + /* Set tol to the square root of the machine precision. */ + /* Unless high precision solutions are required, */ + /* this is the recommended setting. */ + + rc = powell( + &err, + N, /* Dimentionality */ + cp, /* Initial starting point */ + s, /* Size of initial search area */ + 0.00000001, /* Tollerance of error change to stop on */ + 1000, /* Maximum iterations allowed */ + fcn, /* Error function to evaluate */ + NULL, /* Opaque data needed by function */ + progress, /* Progress callback */ + NULL /* Context for callback */ + ); + + + fprintf(stdout,"Status = %d, final approximate solution err = %f:\n",rc,err); + for (j = 0; j < N; j++) { + fprintf(stdout,"cp[%d] = %e, expect %e\n",j,cp[j],expect[j]); + } + + return 0; +} /* main() */ + +/* Function being minimized */ +double fcn( /* Return function value */ +void *fdata, /* Opaque data pointer */ +double tp[] /* Multivriate input value */ +) { + double err, tt; + double temp, temp1, temp2; + int k; + + /* Function Body */ + err = 0.0; + for (k = 0; k < N; ++k) { + temp = (3.0 - 2.0 * tp[k]) * tp[k]; + temp1 = 0.0; + if (k != 0) { + temp1 = tp[k-1]; + } + temp2 = 0.0; + if (k != ((N)-1)) + temp2 = tp[k+1]; + tt = temp - temp1 - 2.0 * temp2 + 1.0; + err += tt * tt; + } + err = sqrt(err); +//printf("Returning %16.14f\n",err); + return err; +} + + + + + + + + + + + + diff --git a/numlib/zbrent.c b/numlib/zbrent.c new file mode 100644 index 0000000..a811818 --- /dev/null +++ b/numlib/zbrent.c @@ -0,0 +1,182 @@ + +/* 1 dimentional root finding code */ +/* inspired by the Van Wijngaarden-Dekker-Brent */ +/* method algorithm presented in */ +/* "Numerical Recipes in C", by W.H.Press, */ +/* B.P.Flannery, S.A.Teukolsky & W.T.Vetterling. */ + +/* + * Copyright 2000 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +#include "numsup.h" +#include "zbrent.h" + +#undef DEBUG + +#define ZBRACK_MAXTRY 40 /* Maximum tries to bracket */ +#define ZBRACK_GOLD 1.618034 /* Golden ratio */ + +/* Bracket search function */ +/* return 0 on sucess */ +/* -1 on no range */ +/* -2 on too many itterations */ +int zbrac( +double *x1p, /* Input and output bracket values */ +double *x2p, /* Min and Max */ +double (*func)(void *fdata, double tp), /* function to evaluate */ +void *fdata /* Opaque data pointer */ +) { + int i; + double x1, x2; /* Bracket under consideration */ + double f1, f2; /* Function values at points x1 and x2 */ + + x1 = *x1p; + x2 = *x2p; + if (x1 == x2) /* Nowhere to go */ + return -1; + + f1 = (*func)(fdata, x1); /* Initial function values */ + f2 = (*func)(fdata, x2); + + for (i = 0; i < ZBRACK_MAXTRY; i++) { + if ((f1 * f2) < 0.0) { + *x1p = x1; + *x2p = x2; + return 0; /* If signs are opposite, we're done */ + } + if (fabs(f2) > fabs(f1)) { /* Move smaller in direction away from larger */ + x1 += ZBRACK_GOLD * (x1 - x2); + f1 = (*func)(fdata, x1); + } else { + x2 += ZBRACK_GOLD * (x2 - x1); + f2 = (*func)(fdata, x2); + } + } + return -2; +} + +#undef ZBRACK_GOLD +#undef ZBRACK_MAXTRY + + +#define ZBRENT_MAXIT 100 + +/* Root finder */ +/* return 0 on sucess */ +/* -1 on root not bracketed */ +/* -2 on too many itterations */ +int zbrent( +double *rv, /* Return value */ +double ax, /* Bracket to search */ +double bx, /* (Min, Max) */ +double tol, /* Desired tollerance */ +double (*func)(void *fdata, double tp), /* function to evaluate */ +void *fdata /* Opaque data pointer */ +) { + int i; + double cx; /* Trial points, bx = best current */ + double af ,bf, cf; /* Function values at those points */ + + af = (*func)(fdata, ax); + bf = (*func)(fdata, bx); + + /* Sanity check bracketing */ + if (af * bf > 0.0) + return -1; /* No good */ + + cx = bx; /* Force bisection for first itter */ + cf = bf; + for (i = 0; i < ZBRENT_MAXIT; i++) { + double xdel; /* Bisection delta to bx */ + double del = 1e80; /* Delta to be applied to bx */ + double pdel = 1e80; /* Last del from interpolation step */ + double tol1; /* Minimum reasonable change in bx */ + + /* Make bx and cx straddle root */ + if (bf * cf > 0.0) { /* bx and cx don't straddle root */ + cx = ax; /* ax must, so make cx = ax */ + cf = af; + pdel = del = bx - ax; + } + + /* Make bx be point closest to solution */ + if (fabs(cf) < fabs(bf)) { + ax = bx; /* swap bx & cx, and make ax == new cx */ + af = bf; + bx = cx; + bf = cf; + cx = ax; + cf = af; + } + tol1 = (0.5 * tol) + (2.0 * DBL_EPSILON * fabs(bx)); /* Minimum tollerable bx move */ + xdel = 0.5 * (cx - bx); /* Delta to bx for bisection move */ + + if (bf == 0.0 || fabs(xdel) <= tol1) { /* If exact soln, or last was min move */ + *rv = bx; + return 0; + } + if (fabs(pdel) >= tol1 && fabs(af) > fabs(bf)) { /* Try inv. quadratic interpolation */ + double P, Q; + + if (ax == cx) { /* Only have 2 points, use extrapolation */ + double R; + R = bf / cf; + P = (cx - bx) * R; + Q = R - 1.0; + } else { /* Brent's interpolation of 3 points */ + double R, S, T; + R = bf / cf; + S = bf / af; + T = af / cf; + P = S * ((T * (R - T) * (cx - bx)) - ((1.0 - R) * (bx - ax))); + Q = (T - 1.0) * (R - 1.0) * (S - 1.0); + } + if (P < 0.0) /* Keep sign of P/Q with abs(P) */ + Q = -Q; + P = fabs(P); + { + double min1, min2; + min1 = (3.0 * xdel * Q) - (tol1 * fabs(Q)); + min2 = fabs(pdel * Q); + if (min2 < min1) + min1 = min2; + + if ((2.0 * P) < min1) { /* Interpolation looks OK */ + pdel = del; /* Remember last delta */ + del = P / Q; /* Next delta */ + } else { + pdel = del = xdel; /* Use bisection */ + } + } + } else { + pdel = del = xdel; /* Use bisection */ + } + ax = bx; /* a keeps previous best point */ + af = bf; + if (fabs(del) > tol1) /* Delta looks reasonable */ + bx += del; + else + bx += (xdel > 0.0 ? tol1 : -tol1); /* Do minimum move in direction of bisection */ + bf = (*func)(fdata, bx); + } + return -2; /* Too many iterations */ +} + + + + + + + + + + + + + + diff --git a/numlib/zbrent.h b/numlib/zbrent.h new file mode 100644 index 0000000..2f8aac4 --- /dev/null +++ b/numlib/zbrent.h @@ -0,0 +1,44 @@ +#ifndef ROOT_H +#define ROOT_H + +/* + * Copyright 2000 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +#ifdef __cplusplus + extern "C" { +#endif + +/* 1 dimentional root finding code */ + +/* Bracket search function */ +/* return 0 on sucess */ +/* -1 on no range */ +/* -2 on too many itterations */ +int zbrac( +double *x1, /* Input and output bracket values */ +double *x2, +double (*func)(void *fdata, double tp), /* function to evaluate */ +void *fdata); /* Opaque data pointer */ + +/* Root finder */ +/* return 0 on sucess */ +/* -1 on root not bracketed */ +/* -2 on too many itterations */ +int zbrent( +double *rv, /* Return value */ +double x1, /* Bracket to search */ +double x2, /* (Min, Max) */ +double tol, /* Desired tollerance */ +double (*func)(void *fdata, double tp), /* function to evaluate */ +void *fdata); /* Opaque data pointer */ + +#ifdef __cplusplus + } +#endif + +#endif /* ROOT_H */ diff --git a/numlib/zbrenttest.c b/numlib/zbrenttest.c new file mode 100644 index 0000000..d7b44d3 --- /dev/null +++ b/numlib/zbrenttest.c @@ -0,0 +1,56 @@ +/* Do a test of the brent 1d root finder */ + +/* + * Copyright 2000 Graeme W. Gill + * All rights reserved. + * + * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :- + * see the License.txt file for licencing details. + */ + +/* Solves (3 - 2*X) * X = -1 */ + +#include "numlib.h" + +int its = 0; +double fcn(void *fdata, double tp); + +/* Expected solution */ +double expect = -2.80776403408306e-01; + +int main(void) +{ + int rv; + double x1, x2, soln; + + /* Attempt bracket the solution */ + x1 = -0.1; + x2 = 0.1; + rv = zbrac(&x1, &x2, fcn, NULL); + if (rv != 0) { + error("zbrack failed with rv = %d\n",rv); + } + printf("Got bracket %f, %f in %d itterations\n",x1,x2,its); + + /* Solve the equation */ + its = 0; + rv = zbrent(&soln, x1, x2, 1e-6, fcn, NULL); + if (rv != 0) { + error("xbrent failed with rv = %d\n",rv); + } + + printf("Solution = %f, expected = %f in %d itterations\n",soln, expect, its); + return 0; + +} /* main() */ + +/* Function being solved */ +double fcn(void *fdata, double tp) +{ + double temp; + its++; + temp = ((3.0 - 2.0 * tp) * tp) + 1.0; +/* printf("~~ %f returns %f\n",tp,temp); */ + return temp; +} + |