diff options
Diffstat (limited to 'consistencyChecker/NMath.cpp')
| -rw-r--r-- | consistencyChecker/NMath.cpp | 664 |
1 files changed, 664 insertions, 0 deletions
diff --git a/consistencyChecker/NMath.cpp b/consistencyChecker/NMath.cpp new file mode 100644 index 0000000..3a58b16 --- /dev/null +++ b/consistencyChecker/NMath.cpp @@ -0,0 +1,664 @@ +// Copyright: Thomas Brox
+
+#include <math.h>
+#include <stdlib.h>
+#include <NMath.h>
+
+namespace NMath {
+
+ const float Pi = 3.1415926536;
+
+ // faculty
+ int faculty(int n) {
+ int aResult = 1;
+ for (int i = 2; i <= n; i++)
+ aResult *= i;
+ return aResult;
+ }
+
+ // binCoeff
+ int binCoeff(const int n, const int k) {
+ if (k > (n >> 1)) return binCoeff(n,n-k);
+ int aResult = 1;
+ for (int i = n; i > (n-k); i--)
+ aResult *= i;
+ for (int j = 2; j <= k; j++)
+ aResult /= j;
+ return aResult;
+ }
+
+ // tangent
+ float tangent(const float x1, const float y1, const float x2, const float y2) {
+ float alpha;
+ float xDiff = x2-x1;
+ float yDiff = y2-y1;
+ if (yDiff > 0) {
+ if (xDiff == 0) alpha = 0.5*Pi;
+ else if (xDiff > 0) alpha = atan(yDiff/xDiff);
+ else alpha = Pi+atan(yDiff/xDiff);
+ }
+ else {
+ if (xDiff == 0) alpha = -0.5*Pi;
+ else if (xDiff > 0) alpha = atan(yDiff/xDiff);
+ else alpha = -Pi+atan(yDiff/xDiff);
+ }
+ return alpha;
+ }
+
+ // absAngleDifference
+ float absAngleDifference(const float aFirstAngle, const float aSecondAngle) {
+ float aAlphaDiff = abs(aFirstAngle - aSecondAngle);
+ if (aAlphaDiff > Pi) aAlphaDiff = 2*Pi-aAlphaDiff;
+ return aAlphaDiff;
+ }
+
+ // angleDifference
+ float angleDifference(const float aFirstAngle, const float aSecondAngle) {
+ float aAlphaDiff = aFirstAngle - aSecondAngle;
+ if (aAlphaDiff > Pi) aAlphaDiff = -2*Pi+aAlphaDiff;
+ else if (aAlphaDiff < -Pi) aAlphaDiff = 2*Pi+aAlphaDiff;
+ return aAlphaDiff;
+ }
+
+ // angleSum
+ float angleSum(const float aFirstAngle, const float aSecondAngle) {
+ float aSum = aFirstAngle + aSecondAngle;
+ if (aSum > Pi) aSum = -2*Pi+aSum;
+ else if (aSum < -Pi) aSum = 2*Pi+aSum;
+ return aSum;
+ }
+
+ // round
+ int round(const float aValue) {
+ float temp1 = floor(aValue);
+ float temp2 = ceil(aValue);
+ if (aValue-temp1 < 0.5) return (int)temp1;
+ else return (int)temp2;
+ }
+
+ // PATransformation
+ // Cyclic Jacobi method for determining the eigenvalues and eigenvectors
+ // of a symmetric matrix.
+ // Ref.: H.R. Schwarz: Numerische Mathematik. Teubner, Stuttgart, 1988.
+ // pp. 243-246.
+ void PATransformation(const CMatrix<float>& aMatrix, CVector<float>& aEigenvalues, CMatrix<float>& aEigenvectors, bool aOrdering) {
+ static const float eps = 0.0001;
+ static const float delta = 0.000001;
+ static const float eps2 = eps*eps;
+ float sum,theta,t,c,r,s,g,h;
+ // Initialization
+ CMatrix<float> aCopy(aMatrix);
+ int n = aEigenvalues.size();
+ aEigenvectors = 0;
+ for (int i = 0; i < n; i++)
+ aEigenvectors(i,i) = 1;
+ // Loop
+ do {
+ // check whether accuracy is reached
+ sum = 0.0;
+ for (int i = 1; i < n; i++)
+ for (int j = 0; j <= i-1; j++)
+ sum += aCopy(i,j)*aCopy(i,j);
+ if (sum+sum > eps2) {
+ for (int p = 0; p < n-1; p++)
+ for (int q = p+1; q < n; q++)
+ if (fabs(aCopy(q,p)) >= eps2) {
+ theta = (aCopy(q,q) - aCopy(p,p)) / (2.0 * aCopy(q,p));
+ t = 1.0;
+ if (fabs(theta) > delta) t = 1.0 / (theta + theta/fabs(theta) * sqrt (theta*theta + 1.0));
+ c = 1.0 / sqrt (1.0 + t*t);
+ s = c*t;
+ r = s / (1.0 + c);
+ aCopy(p,p) -= t * aCopy(q,p);
+ aCopy(q,q) += t * aCopy(q,p);
+ aCopy(q,p) = 0;
+ for (int j = 0; j <= p-1; j++) {
+ g = aCopy(q,j) + r * aCopy(p,j);
+ h = aCopy(p,j) - r * aCopy(q,j);
+ aCopy(p,j) -= s*g;
+ aCopy(q,j) += s*h;
+ }
+ for (int i = p+1; i <= q-1; i++) {
+ g = aCopy(q,i) + r * aCopy(i,p);
+ h = aCopy(i,p) - r * aCopy(q,i);
+ aCopy(i,p) -= s * g;
+ aCopy(q,i) += s * h;
+ }
+ for (int i = q+1; i < n; i++) {
+ g = aCopy(i,q) + r * aCopy(i,p);
+ h = aCopy(i,p) - r * aCopy(i,q);
+ aCopy(i,p) -= s * g;
+ aCopy(i,q) += s * h;
+ }
+ for (int i = 0; i < n; i++) {
+ g = aEigenvectors(i,q) + r * aEigenvectors(i,p);
+ h = aEigenvectors(i,p) - r * aEigenvectors(i,q);
+ aEigenvectors(i,p) -= s * g;
+ aEigenvectors(i,q) += s * h;
+ }
+ }
+ }
+ }
+ // Return eigenvalues
+ while (sum+sum > eps2);
+ for (int i = 0; i < n; i++)
+ aEigenvalues(i) = aCopy(i,i);
+ if (aOrdering) {
+ // Order eigenvalues and eigenvectors
+ for (int i = 0; i < n-1; i++) {
+ int k = i;
+ for (int j = i+1; j < n; j++)
+ if (fabs(aEigenvalues(j)) > fabs(aEigenvalues(k))) k = j;
+ if (k != i) {
+ // Switch eigenvalue i and k
+ float help = aEigenvalues(k);
+ aEigenvalues(k) = aEigenvalues(i);
+ aEigenvalues(i) = help;
+ // Switch eigenvector i and k
+ for (int j = 0; j < n; j++) {
+ help = aEigenvectors(j,k);
+ aEigenvectors(j,k) = aEigenvectors(j,i);
+ aEigenvectors(j,i) = help;
+ }
+ }
+ }
+ }
+ }
+
+ // PABackTransformation
+ void PABacktransformation(const CMatrix<float>& aEigenvectors, const CVector<float>& aEigenvalues, CMatrix<float>& aMatrix) {
+ for (int i = 0; i < aEigenvalues.size(); i++)
+ for (int j = 0; j <= i; j++) {
+ float sum = aEigenvalues(0) * aEigenvectors(i,0) * aEigenvectors(j,0);
+ for (int k = 1; k < aEigenvalues.size(); k++)
+ sum += aEigenvalues(k) * aEigenvectors(i,k) * aEigenvectors(j,k);
+ aMatrix(i,j) = sum;
+ }
+ for (int i = 0; i < aEigenvalues.size(); i++)
+ for (int j = i+1; j < aEigenvalues.size(); j++)
+ aMatrix(i,j) = aMatrix(j,i);
+ }
+
+ // svd (nach Numerical Recipes in C basierend auf Forsythe et al.: Computer Methods for
+ // Mathematical Computations (Englewood Cliffs, NJ: Prentice-Hall), Chapter 9, 1977,
+ // Code übernommen von Bodo Rosenhahn)
+ void svd(CMatrix<float>& U, CMatrix<float>& S, CMatrix<float>& V, bool aOrdering, int aIterations) {
+ static float at,bt,ct;
+ static float maxarg1,maxarg2;
+ #define PYTHAG(a,b) ((at=fabs(a)) > (bt=fabs(b)) ? (ct=bt/at,at*sqrt(1.0+ct*ct)) : (bt ? (ct=at/bt,bt*sqrt(1.0+ct*ct)): 0.0))
+ #define MAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2))
+ #define MIN(a,b) ((a) >(b) ? (b) : (a))
+ #define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
+ int flag,i,its,j,jj,k,l,nm;
+ float c,f,h,s,x,y,z;
+ float anorm=0.0,g=0.0,scale=0.0;
+ int aXSize = U.xSize();
+ int aYSize = U.ySize();
+ CVector<float> aBuffer(aXSize);
+ for (i = 0; i < aXSize; i++) {
+ l=i+1;
+ aBuffer(i)=scale*g;
+ g=s=scale=0.0;
+ if (i < aYSize) {
+ for (k = i; k < aYSize; k++)
+ scale += fabs(U(i,k));
+ if (scale) {
+ for (k = i; k < aYSize; k++) {
+ U(i,k) /= scale;
+ s += U(i,k)*U(i,k);
+ }
+ f = U(i,i);
+ g = -SIGN(sqrt(s),f);
+ h = f*g-s;
+ U(i,i) = f-g;
+ for (j = l; j < aXSize; j++) {
+ for (s = 0.0, k = i; k < aYSize; k++)
+ s += U(i,k)*U(j,k);
+ f = s/h;
+ for (k = i; k < aYSize; k++)
+ U(j,k) += f*U(i,k);
+ }
+ for ( k = i; k < aYSize; k++)
+ U(i,k) *= scale;
+ }
+ }
+ S(i,i) = scale*g;
+ g=s=scale=0.0;
+ if (i < aYSize && i != aXSize-1) {
+ for (k = l; k < aXSize; k++)
+ scale += fabs(U(k,i));
+ if (scale != 0) {
+ for (k = l; k < aXSize; k++) {
+ U(k,i) /= scale;
+ s += U(k,i)*U(k,i);
+ }
+ f = U(l,i);
+ g = -SIGN(sqrt(s),f);
+ h = f*g-s;
+ U(l,i) = f-g;
+ for (k = l; k < aXSize; k++)
+ aBuffer(k) = U(k,i)/h;
+ for (j = l; j < aYSize; j++) {
+ for (s = 0.0, k = l; k < aXSize; k++)
+ s += U(k,j)*U(k,i);
+ for (k = l; k < aXSize; k++)
+ U(k,j) += s*aBuffer(k);
+ }
+ for (k = l; k < aXSize; k++)
+ U(k,i) *= scale;
+ }
+ }
+ anorm = MAX(anorm,(fabs(S(i,i))+fabs(aBuffer(i))));
+ }
+ for (i = aXSize-1; i >= 0; i--) {
+ if (i < aXSize-1) {
+ if (g != 0) {
+ for (j = l; j < aXSize; j++)
+ V(i,j) = U(j,i)/(U(l,i)*g);
+ for (j = l; j < aXSize; j++) {
+ for (s = 0.0, k = l; k < aXSize; k++)
+ s += U(k,i)*V(j,k);
+ for (k = l; k < aXSize; k++)
+ V(j,k) += s*V(i,k);
+ }
+ }
+ for (j = l; j < aXSize; j++)
+ V(j,i) = V(i,j) = 0.0;
+ }
+ V(i,i) = 1.0;
+ g = aBuffer(i);
+ l = i;
+ }
+ for (i = MIN(aYSize-1,aXSize-1); i >= 0; i--) {
+ l = i+1;
+ g = S(i,i);
+ for (j = l; j < aXSize; j++)
+ U(j,i) = 0.0;
+ if (g != 0) {
+ g = 1.0/g;
+ for (j = l; j < aXSize; j++) {
+ for (s = 0.0, k = l; k < aYSize; k++)
+ s += U(i,k)*U(j,k);
+ f = (s/U(i,i))*g;
+ for (k = i; k < aYSize; k++)
+ U(j,k) += f*U(i,k);
+ }
+ for (j = i; j < aYSize; j++)
+ U(i,j) *= g;
+ }
+ else {
+ for (j = i; j < aYSize; j++)
+ U(i,j) = 0.0;
+ }
+ ++U(i,i);
+ }
+ for (k = aXSize-1; k >= 0; k--) {
+ for (its = 1; its <= aIterations; its++) {
+ flag = 1;
+ for (l = k; l > 0; l--) {
+ nm = l - 1;
+ if (fabs(aBuffer(l))+anorm == anorm) {
+ flag = 0; break;
+ }
+ if (fabs(S(nm,nm))+anorm == anorm) break;
+ }
+ if (flag) {
+ c = 0.0;
+ s = 1.0;
+ for (i = l; i <= k; i++) {
+ f = s*aBuffer(i);
+ aBuffer(i) = c*aBuffer(i);
+ if (fabs(f)+anorm == anorm) break;
+ g = S(i,i);
+ h = PYTHAG(f,g);
+ S(i,i) = h;
+ h = 1.0/h;
+ c = g*h;
+ s=-f*h;
+ for (j = 0; j < aYSize; j++) {
+ y = U(nm,j);
+ z = U(i,j);
+ U(nm,j) = y*c + z*s;
+ U(i,j) = z*c - y*s;
+ }
+ }
+ }
+ z = S(k,k);
+ if (l == k) {
+ if (z < 0.0) {
+ S(k,k) = -z;
+ for (j = 0; j < aXSize; j++)
+ V(k,j) = -V(k,j);
+ }
+ break;
+ }
+ if (its == aIterations) std::cerr << "svd: No convergence in " << aIterations << " iterations" << std::endl;
+ x = S(l,l);
+ nm = k-1;
+ y = S(nm,nm);
+ g = aBuffer(nm);
+ h = aBuffer(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 = aBuffer(i);
+ y = S(i,i);
+ h = s*g;
+ g = c*g;
+ z = PYTHAG(f,h);
+ aBuffer(j) = z;
+ float invZ = 1.0/z;
+ c = f*invZ;
+ s = h*invZ;
+ f = x*c+g*s;
+ g = g*c-x*s;
+ h = y*s;
+ y *= c;
+ for (jj = 0; jj < aXSize; jj++) {
+ x = V(j,jj);
+ z = V(i,jj);
+ V(j,jj) = x*c + z*s;
+ V(i,jj) = z*c - x*s;
+ }
+ z = PYTHAG(f,h);
+ S(j,j) = z;
+ if (z != 0) {
+ z = 1.0/z;
+ c = f*z;
+ s = h*z;
+ }
+ f = (c*g)+(s*y);
+ x = (c*y)-(s*g);
+ for (jj = 0; jj < aYSize; jj++) {
+ y = U(j,jj);
+ z = U(i,jj);
+ U(j,jj) = y*c + z*s;
+ U(i,jj) = z*c - y*s;
+ }
+ }
+ aBuffer(l) = 0.0;
+ aBuffer(k) = f;
+ S(k,k) = x;
+ }
+ }
+ // Order singular values
+ if (aOrdering) {
+ for (int i = 0; i < aXSize-1; i++) {
+ int k = i;
+ for (int j = i+1; j < aXSize; j++)
+ if (fabs(S(j,j)) > fabs(S(k,k))) k = j;
+ if (k != i) {
+ // Switch singular value i and k
+ float help = S(k,k);
+ S(k,k) = S(i,i);
+ S(i,i) = help;
+ // Switch columns i and k in U and V
+ for (int j = 0; j < aYSize; j++) {
+ help = U(k,j);
+ U(k,j) = U(i,j);
+ U(i,j) = help;
+ }
+ for (int j = 0; j < aXSize; j++) {
+ help = V(k,j);
+ V(k,j) = V(i,j);
+ V(i,j) = help;
+ }
+ }
+ }
+ }
+ }
+
+ #undef PYTHAG
+ #undef MAX
+ #undef MIN
+ #undef SIGN
+
+ // svdBack
+ void svdBack(CMatrix<float>& U, const CMatrix<float>& S, const CMatrix<float>& V) {
+ for (int y = 0; y < U.ySize(); y++)
+ for (int x = 0; x < U.xSize(); x++)
+ U(x,y) = S(x,x)*U(x,y);
+ U *= trans(V);
+ }
+
+ // Householder-Verfahren (nach Stoer), uebernommen von Bodo Rosenhahn
+ // Bei dem Verfahren wird die Matrix A (hier:*this und die rechte Seite (b)
+ // mit unitaeren Matrizen P multipliziert, so dass A in eine
+ // obere Dreiecksmatrix umgewandelt wird.
+ // Dabei ist P = I + beta * u * uH
+ // Die Vektoren u werden bei jeder Transformation in den nicht
+ // benoetigten unteren Spalten von A gesichert.
+
+ void householder(CMatrix<float>& A, CVector<float>& b) {
+ int i,j,k;
+ float sigma,s,beta,sum;
+ CVector<float> d(A.xSize());
+ for (j = 0; j < A.xSize(); ++j) {
+ sigma = 0;
+ for (i = j; i < A.ySize(); ++i)
+ sigma += A(j,i)*A(j,i);
+ if (sigma == 0) {
+ std::cerr << "NMath::householder(): matrix is singular!" << std::endl;
+ break;
+ }
+ // Choose sign to avoid elimination
+ s = d(j) = A(j,j)<0 ? sqrt(sigma) : -sqrt(sigma);
+ beta = 1.0/(s*A(j,j)-sigma);
+ A(j,j) -= s;
+ // Transform submatrix of A with P
+ for (k = j+1; k < A.xSize(); ++k) {
+ sum = 0.0;
+ for (i = j; i < A.ySize(); ++i)
+ sum += (A(j,i)*A(k,i));
+ sum *= beta;
+ for (i = j; i < A.ySize(); ++i)
+ A(k,i) += A(j,i)*sum;
+ }
+ // Transform right hand side of linear system with P
+ sum = 0.0;
+ for (i = j; i < A.ySize(); ++i)
+ sum += A(j,i)*b(i);
+ sum *= beta;
+ for (i = j; i < A.ySize(); ++i)
+ b(i) += A(j,i)*sum;
+ }
+ for (i = 0; i < A.xSize(); ++i)
+ A(i,i) = d(i);
+ }
+
+ // leastSquares
+ CVector<float> leastSquares(CMatrix<float>& A, CVector<float>& b) {
+ CVector<float> aResult(A.xSize());
+ householder(A,b);
+ for (int i = A.xSize()-1; i >= 0; i--) {
+ float s = 0;
+ for (int k = i+1; k < A.xSize(); k++)
+ s += A(k,i)*aResult(k);
+ aResult(i) = (b(i)-s)/A(i,i);
+ }
+ return aResult;
+ }
+
+ // invRegularized
+ void invRegularized(CMatrix<float>& A, int aReg) {
+ if (A.xSize() != A.ySize()) throw ENonquadraticMatrix(A.xSize(),A.ySize());
+ CVector<float> eVals(A.xSize());
+ CMatrix<float> eVecs(A.xSize(),A.ySize());
+ PATransformation(A,eVals,eVecs);
+ for (int i = 0 ; i < A.xSize(); i++)
+ if (eVals(i) < aReg) eVals(i) = 1.0/aReg;
+ else eVals(i) = 1.0/eVals(i);
+ PABacktransformation(eVecs,eVals,A);
+ }
+
+ // cholesky
+ void cholesky(CMatrix<float>& A) {
+ if (A.xSize() != A.ySize()) throw ENonquadraticMatrix(A.xSize(),A.ySize());
+ CVector<float> d(A.xSize());
+ for (int i = 0; i < A.xSize(); i++)
+ for (int j = i; j < A.ySize(); j++) {
+ float sum = A(j,i);
+ for (int k = i-1; k >= 0; k--)
+ sum -= A(k,i)*A(k,j);
+ if (i == j) {
+ if (sum <= 0.0) return;//throw ENonPositiveDefinite();
+ d(i) = sqrt(sum);
+ }
+ else A(i,j) = sum/d(i);
+ }
+ for (int i = 0; i < A.xSize(); i++)
+ A(i,i) = d(i);
+ }
+
+ // triangularSolve
+ void triangularSolve(CMatrix<float>& L, CVector<float>& aIn, CVector<float>& aOut) {
+ for (int i = 0; i < aIn.size(); i++) {
+ float sum = aIn(i);
+ for (int j = 0; j < i; j++)
+ sum -= L(j,i)*aOut(j);
+ aOut(i) = sum/L(i,i);
+ }
+ }
+
+ void triangularSolve(CMatrix<float>& L, CMatrix<float>& aIn, CMatrix<float>& aOut) {
+ CVector<float> invLii(aIn.xSize());
+ for (int i = 0; i < aIn.xSize(); i++)
+ invLii(i) = 1.0/L(i,i);
+ for (int k = 0; k < aIn.ySize(); k++)
+ for (int i = 0; i < aIn.xSize(); i++) {
+ float sum = aIn(i,k);
+ for (int j = 0; j < i; j++)
+ sum -= L(j,i)*aOut(j,k);
+ aOut(i,k) = sum*invLii(i);
+ }
+ }
+
+ // triangularSolveTransposed
+ void triangularSolveTransposed(CMatrix<float>& L, CVector<float>& aIn, CVector<float>& aOut) {
+ for (int i = aIn.size()-1; i >= 0; i--) {
+ float sum = aIn(i);
+ for (int j = aIn.size()-1; j > i; j--)
+ sum -= L(i,j)*aOut(j);
+ aOut(i) = sum/L(i,i);
+ }
+ }
+
+ void triangularSolveTransposed(CMatrix<float>& L, CMatrix<float>& aIn, CMatrix<float>& aOut) {
+ CVector<float> invLii(aIn.xSize());
+ for (int i = 0; i < aIn.xSize(); i++)
+ invLii(i) = 1.0/L(i,i);
+ for (int k = 0; k < aIn.ySize(); k++)
+ for (int i = aIn.xSize()-1; i >= 0; i--) {
+ float sum = aIn(i,k);
+ for (int j = aIn.xSize()-1; j > i; j--)
+ sum -= L(i,j)*aOut(j,k);
+ aOut(i,k) = sum*invLii(i);
+ }
+ }
+
+ // choleskyInv
+ void choleskyInv(const CMatrix<float>& L, CMatrix<float>& aInv) {
+ aInv = 0;
+ // Compute the inverse of L
+ CMatrix<float> invL(L.xSize(),L.ySize());
+ for (int i = 0; i < L.xSize(); i++)
+ invL(i,i) = 1.0/L(i,i);
+ for (int i = 0; i < L.xSize(); i++)
+ for (int j = i+1; j < L.ySize(); j++) {
+ float sum = 0.0;
+ for (int k = i; k < j; k++)
+ sum -= L(k,j)*invL(i,k);
+ invL(i,j) = sum*invL(j,j);
+ }
+ // Compute lower triangle of aInv = invL^T * invL
+ for (int i = 0; i < aInv.xSize(); i++)
+ for (int j = i; j < aInv.ySize(); j++) {
+ float sum = 0.0;
+ for (int k = j; k < aInv.ySize(); k++)
+ sum += invL(j,k)*invL(i,k);
+ aInv(i,j) = sum;
+ }
+ // Complete aInv
+ for (int i = 0; i < aInv.xSize(); i++)
+ for (int j = i+1; j < aInv.ySize(); j++)
+ aInv(j,i) = aInv(i,j);
+ }
+
+ // eulerAngles
+ void eulerAngles(float rx, float ry, float rz, CMatrix<float>& A) {
+ CMatrix<float> Rx(4,4,0);
+ CMatrix<float> Ry(4,4,0);
+ CMatrix<float> Rz(4,4,0);
+ Rx(0,0)=1.0;Rx(1,1)=cos(rx);Rx(2,2)=cos(rx);Rx(3,3)=1.0;
+ Rx(2,1)=-sin(rx);Rx(1,2)=sin(rx);
+ Ry(1,1)=1.0;Ry(0,0)=cos(ry);Ry(2,2)=cos(ry);Ry(3,3)=1.0;
+ Ry(0,2)=-sin(ry);Ry(2,0)=sin(ry);
+ Rz(2,2)=1.0;Rz(0,0)=cos(rz);Rz(1,1)=cos(rz);Rz(3,3)=1.0;
+ Rz(1,0)=-sin(rz);Rz(0,1)=sin(rz);
+ A=Rz*Ry*Rx;
+ }
+
+ // RBM2Twist
+ void RBM2Twist(CVector<float>& T, CMatrix<float>& fRBM) {
+ T.setSize(6);
+ CMatrix<double> dRBM(4,4);
+ for (int i = 0; i < 16; i++)
+ dRBM.data()[i] = fRBM.data()[i];
+ CVector<double> omega;
+ double theta;
+ CVector<double> v;
+ CMatrix<double> R(3,3);
+ double sum = 0.0;
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ if (i != j) sum += dRBM(i,j)*dRBM(i,j);
+ else sum += (dRBM(i,i)-1.0)*(dRBM(i,i)-1.0);
+ if (sum < 0.0000001) {
+ T(0)=fRBM(3,0); T(1)=fRBM(3,1); T(2)=fRBM(3,2);
+ T(3)=0.0; T(4)=0.0; T(5)=0.0;
+ }
+ else {
+ double diag = (dRBM(0,0)+dRBM(1,1)+dRBM(2,2)-1.0)*0.5;
+ if (diag < -1.0) diag = -1.0;
+ else if (diag > 1.0) diag = 1.0;
+ theta = acos(diag);
+ if (sin(theta)==0) theta += 0.0000001;
+ omega.setSize(3);
+ omega(0)=(dRBM(1,2)-dRBM(2,1));
+ omega(1)=(dRBM(2,0)-dRBM(0,2));
+ omega(2)=(dRBM(0,1)-dRBM(1,0));
+ omega*=(1.0/(2.0*sin(theta)));
+ CMatrix<double> omegaHat(3,3);
+ omegaHat.data()[0] = 0.0; omegaHat.data()[1] = -omega(2); omegaHat.data()[2] = omega(1);
+ omegaHat.data()[3] = omega(2); omegaHat.data()[4] = 0.0; omegaHat.data()[5] = -omega(0);
+ omegaHat.data()[6] = -omega(1); omegaHat.data()[7] = omega(0); omegaHat.data()[8] = 0.0;
+ CMatrix<double> omegaT(3,3);
+ for (int j = 0; j < 3; j++)
+ for (int i = 0; i < 3; i++)
+ omegaT(i,j) = omega(i)*omega(j);
+ R = (omegaHat*(double)sin(theta))+((omegaHat*omegaHat)*(double)(1.0-cos(theta)));
+ R(0,0) += 1.0; R(1,1) += 1.0; R(2,2) += 1.0;
+ CMatrix<double> A(3,3);
+ A.fill(0.0);
+ A(0,0)=1.0; A(1,1)=1.0; A(2,2)=1.0;
+ A -= R; A*=omegaHat; A+=omegaT*theta;
+ CVector<double> p(3);
+ p(0)=dRBM(3,0);
+ p(1)=dRBM(3,1);
+ p(2)=dRBM(3,2);
+ A.inv();
+ v=A*p;
+ T(0) = (float)(v(0)*theta);
+ T(1) = (float)(v(1)*theta);
+ T(2) = (float)(v(2)*theta);
+ T(3) = (float)(theta*omega(0));
+ T(4) = (float)(theta*omega(1));
+ T(5) = (float)(theta*omega(2));
+ }
+ }
+
+}
+
|