From 13e123eb104ceadc377ee32be2a96bb574c08d94 Mon Sep 17 00:00:00 2001 From: cam Date: Sat, 8 Oct 2016 14:48:35 -0600 Subject: First commit --- video_input/consistencyChecker/NMath.cpp | 664 +++++++++++++++++++++++++++++++ 1 file changed, 664 insertions(+) create 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 new file mode 100644 index 0000000..3a58b16 --- /dev/null +++ b/video_input/consistencyChecker/NMath.cpp @@ -0,0 +1,664 @@ +// 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