Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
path: root/intern/iksolver/intern/TNT/svd.h
diff options
context:
space:
mode:
Diffstat (limited to 'intern/iksolver/intern/TNT/svd.h')
-rw-r--r--intern/iksolver/intern/TNT/svd.h507
1 files changed, 507 insertions, 0 deletions
diff --git a/intern/iksolver/intern/TNT/svd.h b/intern/iksolver/intern/TNT/svd.h
new file mode 100644
index 00000000000..9be2d98bd9f
--- /dev/null
+++ b/intern/iksolver/intern/TNT/svd.h
@@ -0,0 +1,507 @@
+/**
+ * $Id$
+ * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version. The Blender
+ * Foundation also sells licenses for use in proprietary software under
+ * the Blender License. See http://www.blender.org/BL/ for information
+ * about this.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
+ * All rights reserved.
+ *
+ * The Original Code is: all of this file.
+ *
+ * Contributor(s): none yet.
+ *
+ * ***** END GPL/BL DUAL LICENSE BLOCK *****
+ */
+
+#ifndef SVD_H
+
+#define SVD_H
+
+// Compute the Single Value Decomposition of an arbitrary matrix A
+// That is compute the 3 matrices U,W,V with U column orthogonal (m,n)
+// ,W a diagonal matrix and V an orthogonal square matrix s.t.
+// A = U.W.Vt. From this decomposition it is trivial to compute the
+// inverse of A as Ainv = V.Winv.tranpose(U).
+// work_space is a temporary vector used by this class to compute
+// intermediate values during the computation of the SVD. This should
+// be of length a.num_cols. This is not checked
+
+#include "tntmath.h"
+
+namespace TNT
+{
+
+
+template <class MaTRiX, class VecToR >
+void SVD(MaTRiX &a, VecToR &w, MaTRiX &v, VecToR &work_space) {
+
+ int n = a.num_cols();
+ int m = a.num_rows();
+
+ int flag,i,its,j,jj,k,l(0),nm(0);
+ typename MaTRiX::value_type c,f,h,x,y,z;
+ typename MaTRiX::value_type anorm(0),g(0),scale(0);
+ typename MaTRiX::value_type s(0);
+
+ work_space.newsize(n);
+
+ for (i=1;i<=n;i++) {
+ l=i+1;
+ work_space(i)=scale*g;
+
+ g = (typename MaTRiX::value_type)0;
+
+ s = (typename MaTRiX::value_type)0;
+ scale = (typename MaTRiX::value_type)0;
+
+ if (i <= m) {
+ for (k=i;k<=m;k++) scale += TNT::abs(a(k,i));
+ if (scale > (typename MaTRiX::value_type)0) {
+ for (k=i;k<=m;k++) {
+ a(k,i) /= scale;
+ s += a(k,i)*a(k,i);
+ }
+ f=a(i,i);
+ g = -TNT::sign(sqrt(s),f);
+ h=f*g-s;
+ a(i,i)=f-g;
+ if (i != n) {
+ for (j=l;j<=n;j++) {
+ s = (typename MaTRiX::value_type)0;
+ for (k=i;k<=m;k++) s += a(k,i)*a(k,j);
+ f=s/h;
+ for (k=i;k<=m;k++) a(k,j) += f*a(k,i);
+ }
+ }
+ for (k=i;k<=m;k++) a(k,i) *= scale;
+ }
+ }
+ w(i)=scale*g;
+ g = (typename MaTRiX::value_type)0;
+ s = (typename MaTRiX::value_type)0;
+ scale = (typename MaTRiX::value_type)0;
+ if (i <= m && i != n) {
+ for (k=l;k<=n;k++) scale += TNT::abs(a(i,k));
+ if (scale > (typename MaTRiX::value_type)0) {
+ for (k=l;k<=n;k++) {
+ a(i,k) /= scale;
+ s += a(i,k)*a(i,k);
+ }
+ f=a(i,l);
+ g = -TNT::sign(sqrt(s),f);
+ h=f*g-s;
+ a(i,l)=f-g;
+ for (k=l;k<=n;k++) work_space(k)=a(i,k)/h;
+ if (i != m) {
+ for (j=l;j<=m;j++) {
+ s = (typename MaTRiX::value_type)0;
+ for (k=l;k<=n;k++) s += a(j,k)*a(i,k);
+ for (k=l;k<=n;k++) a(j,k) += s*work_space(k);
+ }
+ }
+ for (k=l;k<=n;k++) a(i,k) *= scale;
+ }
+ }
+ anorm=TNT::max(anorm,(TNT::abs(w(i))+TNT::abs(work_space(i))));
+ }
+ for (i=n;i>=1;i--) {
+ if (i < n) {
+ if (g != (typename MaTRiX::value_type)0) {
+ for (j=l;j<=n;j++)
+ v(j,i)=(a(i,j)/a(i,l))/g;
+ for (j=l;j<=n;j++) {
+ s = (typename MaTRiX::value_type)0;
+ for (k=l;k<=n;k++) s += a(i,k)*v(k,j);
+ for (k=l;k<=n;k++) v(k,j) += s*v(k,i);
+ }
+ }
+ for (j=l;j<=n;j++) v(i,j)=v(j,i)= (typename MaTRiX::value_type)0;
+ }
+ v(i,i)= (typename MaTRiX::value_type)1;
+ g=work_space(i);
+ l=i;
+ }
+ for (i=n;i>=1;i--) {
+ l=i+1;
+ g=w(i);
+ if (i < n) {
+ for (j=l;j<=n;j++) a(i,j)= (typename MaTRiX::value_type)0;
+ }
+ if (g != (typename MaTRiX::value_type)0) {
+ g= ((typename MaTRiX::value_type)1)/g;
+ if (i != n) {
+ for (j=l;j<=n;j++) {
+ s = (typename MaTRiX::value_type)0;
+ for (k=l;k<=m;k++) s += a(k,i)*a(k,j);
+ f=(s/a(i,i))*g;
+ for (k=i;k<=m;k++) a(k,j) += f*a(k,i);
+ }
+ }
+ for (j=i;j<=m;j++) a(j,i) *= g;
+ } else {
+ for (j=i;j<=m;j++) a(j,i)= (typename MaTRiX::value_type)0;
+ }
+ ++a(i,i);
+ }
+ for (k=n;k>=1;k--) {
+ for (its=1;its<=30;its++) {
+ flag=1;
+ for (l=k;l>=1;l--) {
+ nm=l-1;
+ if (TNT::abs(work_space(l))+anorm == anorm) {
+ flag=0;
+ break;
+ }
+ if (TNT::abs(w(nm))+anorm == anorm) break;
+ }
+ if (flag) {
+ c= (typename MaTRiX::value_type)0;
+ s= (typename MaTRiX::value_type)1;
+ for (i=l;i<=k;i++) {
+ f=s*work_space(i);
+ if (TNT::abs(f)+anorm != anorm) {
+ g=w(i);
+ h= (typename MaTRiX::value_type)TNT::pythag(float(f),float(g));
+ w(i)=h;
+ h= ((typename MaTRiX::value_type)1)/h;
+ c=g*h;
+ s=(-f*h);
+ for (j=1;j<=m;j++) {
+ y=a(j,nm);
+ z=a(j,i);
+ a(j,nm)=y*c+z*s;
+ a(j,i)=z*c-y*s;
+ }
+ }
+ }
+ }
+ z=w(k);
+ if (l == k) {
+ if (z < (typename MaTRiX::value_type)0) {
+ w(k) = -z;
+ for (j=1;j<=n;j++) v(j,k)=(-v(j,k));
+ }
+ break;
+ }
+
+
+#if 1
+ if (its == 30)
+ {
+ TNTException an_exception;
+ an_exception.i = 0;
+ throw an_exception;
+
+ return ;
+ assert(false);
+ }
+#endif
+ x=w(l);
+ nm=k-1;
+ y=w(nm);
+ g=work_space(nm);
+ h=work_space(k);
+ f=((y-z)*(y+z)+(g-h)*(g+h))/(((typename MaTRiX::value_type)2)*h*y);
+ g=(typename MaTRiX::value_type)TNT::pythag(float(f), float(1));
+ f=((x-z)*(x+z)+h*((y/(f+TNT::sign(g,f)))-h))/x;
+ c = (typename MaTRiX::value_type)1;
+ s = (typename MaTRiX::value_type)1;
+ for (j=l;j<=nm;j++) {
+ i=j+1;
+ g=work_space(i);
+ y=w(i);
+ h=s*g;
+ g=c*g;
+ z=(typename MaTRiX::value_type)TNT::pythag(float(f),float(h));
+ work_space(j)=z;
+ c=f/z;
+ s=h/z;
+ f=x*c+g*s;
+ g=g*c-x*s;
+ h=y*s;
+ y=y*c;
+ for (jj=1;jj<=n;jj++) {
+ x=v(jj,j);
+ z=v(jj,i);
+ v(jj,j)=x*c+z*s;
+ v(jj,i)=z*c-x*s;
+ }
+ z=(typename MaTRiX::value_type)TNT::pythag(float(f),float(h));
+ w(j)=z;
+ if (z != (typename MaTRiX::value_type)0) {
+ z= ((typename MaTRiX::value_type)1)/z;
+ c=f*z;
+ s=h*z;
+ }
+ f=(c*g)+(s*y);
+ x=(c*y)-(s*g);
+ for (jj=1;jj<=m;jj++) {
+ y=a(jj,j);
+ z=a(jj,i);
+ a(jj,j)=y*c+z*s;
+ a(jj,i)=z*c-y*s;
+ }
+ }
+ work_space(l)= (typename MaTRiX::value_type)0;
+ work_space(k)=f;
+ w(k)=x;
+ }
+ }
+};
+
+
+
+// A is replaced by the column orthogonal matrix U
+
+
+template <class MaTRiX, class VecToR >
+void SVD_a( MaTRiX &a, VecToR &w, MaTRiX &v) {
+
+ int n = a.num_cols();
+ int m = a.num_rows();
+
+ int flag,i,its,j,jj,k,l,nm;
+ typename MaTRiX::value_type anorm,c,f,g,h,s,scale,x,y,z;
+
+ VecToR work_space;
+ work_space.newsize(n);
+
+ g = scale = anorm = 0.0;
+
+ for (i=1;i <=n;i++) {
+ l = i+1;
+ work_space(i) = scale*g;
+ g = s=scale=0.0;
+
+ if (i <= m) {
+ for(k=i; k<=m; k++) scale += abs(a(k,i));
+
+ if (scale) {
+ for (k = i; k <=m ; k++) {
+ a(k,i) /= scale;
+ s += a(k,i)*a(k,i);
+ }
+ f = a(i,i);
+ g = -sign(sqrt(s),f);
+ h = f*g -s;
+ a(i,i) = f-g;
+
+ for (j = l; j <=n; j++) {
+ for (s = 0.0,k =i;k<=m;k++) s += a(k,i)*a(k,j);
+ f = s/h;
+ for (k = i; k <= m; k++) a(k,j) += f*a(k,i);
+ }
+ for (k = i; k <=m;k++) a(k,i) *= scale;
+ }
+ }
+
+ w(i) = scale*g;
+ g = s = scale = 0.0;
+
+ if (i <=m && i != n) {
+ for (k = l; k <=n;k++) scale += abs(a(i,k));
+ if (scale) {
+ for(k = l;k <=n;k++) {
+ a(i,k) /= scale;
+ s += a(i,k) * a(i,k);
+ }
+
+ f = a(i,l);
+ g = -sign(sqrt(s),f);
+ h= f*g -s;
+ a(i,l) = f-g;
+ for(k=l;k<=n;k++) work_space(k) = a(i,k)/h;
+ for (j=l;j<=m;j++) {
+ for(s = 0.0,k=l;k<=n;k++) s+= a(j,k)*a(i,k);
+ for(k=l;k<=n;k++) a(j,k) += s*work_space(k);
+ }
+ for(k=l;k<=n;k++) a(i,k)*=scale;
+ }
+ }
+ anorm = max(anorm,(abs(w(i)) + abs(work_space(i))));
+ }
+ for (i=n;i>=1;i--) {
+ if (i <n) {
+ if (g) {
+ for(j=l;j<=n;j++) v(j,i) = (a(i,j)/a(i,l))/g;
+ for(j=l;j<=n;j++) {
+ for(s=0.0,k=l;k<=n;k++) s += a(i,k)*v(k,j);
+ for(k=l; k<=n;k++) v(k,j) +=s*v(k,i);
+ }
+ }
+ for(j=l;j <=n;j++) v(i,j) = v(j,i) = 0.0;
+ }
+ v(i,i) = 1.0;
+ g = work_space(i);
+ l = i;
+ }
+
+ for (i = min(m,n);i>=1;i--) {
+ l = i+1;
+ g = w(i);
+ for (j=l;j <=n;j++) a(i,j) = 0.0;
+ if (g) {
+ g = 1.0/g;
+ for (j=l;j<=n;j++) {
+ for (s = 0.0,k=l;k<=m;k++) s += a(k,i)*a(k,j);
+ f = (s/a(i,i))*g;
+ for (k=i;k<=m;k++) a(k,j) += f*a(k,i);
+ }
+ for (j=i;j<=m;j++) a(j,i)*=g;
+ } else {
+ for (j=i;j<=m;j++) a(j,i) = 0.0;
+ }
+ ++a(i,i);
+ }
+
+ for (k=n;k>=1;k--) {
+ for (its=1;its<=30;its++) {
+ flag=1;
+ for(l=k;l>=1;l--) {
+ nm = l-1;
+ if (abs(work_space(l)) + anorm == anorm) {
+ flag = 0;
+ break;
+ }
+ if (abs(w(nm)) + anorm == anorm) break;
+ }
+ if (flag) {
+ c = 0.0;
+ s = 1.0;
+ for (i=l;i<=k;i++) {
+ f = s*work_space(i);
+ work_space(i) = c*work_space(i);
+ if (abs(f) +anorm == anorm) break;
+ g = w(i);
+ h = pythag(f,g);
+ w(i) = h;
+ h = 1.0/h;
+ c = g*h;
+ s = -f*h;
+ for (j=1;j<=m;j++) {
+ y= a(j,nm);
+ z=a(j,i);
+ a(j,nm) = y*c + z*s;
+ a(j,i) = z*c - y*s;
+ }
+ }
+ }
+ z=w(k);
+ if (l==k) {
+ if (z <0.0) {
+ w(k) = -z;
+ for (j=1;j<=n;j++) v(j,k) = -v(j,k);
+ }
+ break;
+ }
+
+ if (its == 30) assert(false);
+
+ x=w(l);
+ nm=k-1;
+ y=w(nm);
+ g=work_space(nm);
+ h=work_space(k);
+
+ f= ((y-z)*(y+z) + (g-h)*(g+h))/(2.0*h*y);
+ g = pythag(f,1.0);
+ f= ((x-z)*(x+z) + h*((y/(f + sign(g,f)))-h))/x;
+ c=s=1.0;
+
+ for (j=l;j<=nm;j++) {
+ i=j+1;
+ g = work_space(i);
+ y=w(i);
+ h=s*g;
+ g=c*g;
+ z=pythag(f,h);
+ work_space(j) = z;
+ c=f/z;
+ s=h/z;
+ f=x*c + g*s;
+ g= g*c - x*s;
+ h=y*s;
+ y*=c;
+ for(jj=1;jj<=n;jj++) {
+ x=v(jj,j);
+ z=v(jj,i);
+ v(jj,j) = x*c + z*s;
+ v(jj,i) = z*c- x*s;
+ }
+ z=pythag(f,h);
+ w(j)=z;
+ if(z) {
+ z = 1.0/z;
+ c=f*z;
+ s=h*z;
+ }
+ f=c*g + s*y;
+ x= c*y-s*g;
+
+ for(jj=1;jj<=m;jj++) {
+ y=a(jj,j);
+ z=a(jj,i);
+ a(jj,j) = y*c+z*s;
+ a(jj,i) = z*c - y*s;
+ }
+ }
+
+ work_space(l) = 0.0;
+ work_space(k) = f;
+ w(k) = x;
+ }
+ }
+}
+}
+#endif
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-17 07:20:39 +0300