From af50ae73cfe6f1533e5390e5131e25c6d322dc8a Mon Sep 17 00:00:00 2001 From: cam Date: Wed, 4 Jan 2017 23:09:30 -0700 Subject: removed .cpp and .h files --- video_input/consistencyChecker/NMath.cpp | 664 ------------------------------- 1 file changed, 664 deletions(-) delete mode 100644 video_input/consistencyChecker/NMath.cpp (limited to 'video_input/consistencyChecker/NMath.cpp') diff --git a/video_input/consistencyChecker/NMath.cpp b/video_input/consistencyChecker/NMath.cpp deleted file mode 100644 index 3a58b16..0000000 --- a/video_input/consistencyChecker/NMath.cpp +++ /dev/null @@ -1,664 +0,0 @@ -// Copyright: Thomas Brox - -#include -#include -#include - -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& aMatrix, CVector& aEigenvalues, CMatrix& 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 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& aEigenvectors, const CVector& aEigenvalues, CMatrix& 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& U, CMatrix& S, CMatrix& 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 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& U, const CMatrix& S, const CMatrix& 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& A, CVector& b) { - int i,j,k; - float sigma,s,beta,sum; - CVector 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 leastSquares(CMatrix& A, CVector& b) { - CVector 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& A, int aReg) { - if (A.xSize() != A.ySize()) throw ENonquadraticMatrix(A.xSize(),A.ySize()); - CVector eVals(A.xSize()); - CMatrix 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& A) { - if (A.xSize() != A.ySize()) throw ENonquadraticMatrix(A.xSize(),A.ySize()); - CVector 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& L, CVector& aIn, CVector& 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& L, CMatrix& aIn, CMatrix& aOut) { - CVector 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& L, CVector& aIn, CVector& 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& L, CMatrix& aIn, CMatrix& aOut) { - CVector 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& L, CMatrix& aInv) { - aInv = 0; - // Compute the inverse of L - CMatrix 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& A) { - CMatrix Rx(4,4,0); - CMatrix Ry(4,4,0); - CMatrix 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& T, CMatrix& fRBM) { - T.setSize(6); - CMatrix dRBM(4,4); - for (int i = 0; i < 16; i++) - dRBM.data()[i] = fRBM.data()[i]; - CVector omega; - double theta; - CVector v; - CMatrix 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 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 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 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 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)); - } - } - -} - -- cgit v1.2.3-70-g09d2