void matrixNormalTransform(matrix* r, matrix m) { m.v[3] = 0; m.v[7] = 0; m.v[11] = 0; m.v[15] = 1; matrix t; matrixInverse( &t, m ); matrixTranspose( r, t ); }
void Frustum::Extract(const Matrix44f &view, const Projection &proj) { static const int LBN = kLBNCorner, LTN = kLTNCorner, LBF = kLBFCorner, LTF = kLTFCorner, RBN = kRBNCorner, RTN = kRTNCorner, RBF = kRBFCorner, RTF = kRTFCorner; const float lnear = -proj.get_near(); const float lfar = -proj.get_far(); const Vec3f nearNorm(0.0f, 0.0f, -1.0f); const Vec3f farNorm(0.0f, 0.0f, 1.0f); // kLeftPlane plane const Vec3f ltn(proj.get_left(), proj.get_top(), lnear), lbn(proj.get_left(), proj.get_bottom(), lnear), ltf(proj.get_far_left(), proj.get_far_top(), lfar), lbf(proj.get_far_left(), proj.get_far_bottom(), lfar); const Vec3f leftNorm = vecNormal( vecCross(lbn - ltn, ltf - ltn)); // kRightPlane plane const Vec3f rtn(proj.get_right(), proj.get_top(), lnear), // right top near rbn(proj.get_right(), proj.get_bottom(), lnear), // right bottom near rtf(proj.get_far_right(), proj.get_far_top(), lfar); // right top far const Vec3f rightNorm = vecNormal(vecCross(rtf - rtn, rbn - rtn)); // kTopPlane plane const Vec3f topNorm = vecNormal(vecCross(ltf - ltn, rtn - ltn)); // kBottomPlane plane lbn rbf const Vec3f rbf(proj.get_far_right(), proj.get_far_bottom(), lfar); const Vec3f botNorm = vecNormal(vecCross(rbf - rbn, lbn - rbn)); Matrix44f viewSpace, viewSpaceN; matrixInverse(view, &viewSpace); viewSpaceN = matrixTranspose(view); m_conners[LBN] = lbn * viewSpace; m_conners[LTN] = ltn * viewSpace; m_conners[LBF] = lbf * viewSpace; m_conners[LTF] = ltf * viewSpace; m_conners[RBN] = rbn * viewSpace; m_conners[RTN] = rtn * viewSpace; m_conners[RBF] = rbf * viewSpace; m_conners[RTF] = rtf * viewSpace; m_planes[kLeftPlane] = Planef(leftNorm * viewSpaceN, m_conners[LTN]); m_planes[kRightPlane] = Planef(rightNorm * viewSpaceN, m_conners[RTN]); m_planes[kTopPlane] = Planef(topNorm * viewSpaceN, m_conners[RTN]); m_planes[kBottomPlane] = Planef(botNorm * viewSpaceN, m_conners[LBN]); m_planes[kNearPlane] = Planef(nearNorm * viewSpaceN, m_conners[LBN]); m_planes[kFarPlane] = Planef(farNorm * viewSpaceN, m_conners[RTF]); }
/* Main method to test math */ int main() { // Test matrix math struct Matrix a; //double valuesa[9] = { 5.0, -2.0, 1.0, 0.0, 3.0, -1.0, 2.0, 0.0, 7.0 }; double valuesa[16] = { 1.0, 3.0, -2.0, 1.0, 5.0, 1.0, 0.0, -1.0, 0.0, 1.0, 0.0, -2.0, 2.0, -1.0, 0.0, 3.0 }; newMatrix(&a, valuesa, 4, 4); struct Matrix b; double valuesb[9] = { 1.0, 2.0, 3.0, 0.0, -4.0, 1.0, 0.0, 3.0, -1.0 }; newMatrix(&b, valuesb, 3, 3); struct Matrix cofa; matrixCofactor(&cofa, a); printf("A Cofactor=\n"); matrixToString(cofa); struct Matrix inva; matrixInverse(&inva, a); printf("A Inverse=\n"); matrixToString(inva); printf("A=\n"); matrixToString(a); printf("B=\n"); matrixToString(b); printf("A is %sa square matrix.\n", matrixIsSquare(a) ? "" : "not "); printf("B is %sa square matrix.\n", matrixIsSquare(b) ? "" : "not "); printf("The determinant of A is %f.\n", matrixDeterminant(a)); printf("A is %sunique.\n", matrixIsUnique(a) ? "" : "not "); printf("The determinant of B is %f.\n", matrixDeterminant(b)); printf("B is %sunique.\n", matrixIsUnique(b) ? "" : "not "); struct Matrix aplusb; matrixAdd(&aplusb, a, b); printf("A+B=\n"); matrixToString(aplusb); struct Matrix asubb; matrixSubtract(&asubb, a, b); printf("A-B=\n"); matrixToString(asubb); struct Matrix ascale; matrixScale(&ascale, a, 5); printf("5A=\n"); matrixToString(ascale); struct Matrix bscale; matrixScale(&bscale, b, 5); printf("5B=\n"); matrixToString(bscale); printf("A and B are %sthe same size.\n", matrixIsSameSize(a, b) ? "" : "not "); printf("A=\n"); matrixToString(a); printf("B=\n"); matrixToString(b); // Test 3D vector math }
Matrixf44 Matrixf44::getTranpose() { if (transposeReady == false) { for (int j = 0; j < 4; ++j) for (int i = 0; i < 4; ++i) transpose[i][j] = _matrix[j][i]; transposeReady = true; } Matrixf44 matrixInverse(transpose); return matrixInverse; }
void testMatrixInverse(){ double **A, **B; int n; n=3; A=allocateDoubleMatrix(3, 3); A[0][0]=1.0; A[0][1]=2.0; A[0][2]=3.0; A[1][0]=0.0; A[1][1]=4.0; A[1][2]=5.0; A[2][0]=1.0; A[2][1]=0.0; A[2][2]=6.0; B=matrixInverse(A,n); fprintf(stdout,"Matrix Inversion\n"); printMatrix(B,n,n); }
void EigenGraspInterface::computeProjectionMatrices() { if (mP) delete mP; if (mPInv) delete mPInv; //first build the E matrix that just contains the (potentially non-orthonormal) bases Matrix E(eSize, dSize); for (int e=0; e<eSize; e++) { for (int d=0; d<dSize; d++) { E.elem(e,d) = mGrasps[e]->getAxisValue(d); } } //the trivial case (assumes ortho-normal E) //mP = new Matrix(E); //mPInv = new Matrix(E.transposed()); //general case //remember: P(PInv(a)) = a (always) // PInv(P(x)) != x (usually) // P = (E*ET)'*E // P' = ET Matrix ET(E.transposed()); Matrix EET(eSize, eSize); matrixMultiply(E, ET, EET); Matrix EETInv(eSize, eSize); int result = matrixInverse(EET, EETInv); if (result) { DBGA("Projection matrix is rank deficient!"); mP = new Matrix(Matrix::ZEROES<Matrix>(eSize, dSize)); mPInv = new Matrix(Matrix::ZEROES<Matrix>(dSize, eSize)); return; } mP = new Matrix(eSize, dSize); matrixMultiply(EETInv, E, *mP); mPInv = new Matrix(ET); }
// Parse the root file located at path and parse the Ttree treeName. // prefix identifies the track which is to be used, e.g. // trk_, trkSiSpSeeded_, pseudoTrk_ int EventReader::parse (const char* path, const char* treeName, const char* prefix) { TFile *file; TTree *tree; // Open data file and read TTree file = new TFile (path, "READ", "TrackFitData Access"); if (!file) { std::cerr << "Could not open " << path << std::endl; return -1; } tree = ( TTree* )file->Get (treeName); if (!tree) { std::cerr << "Could not get tree " << treeName << std::endl; delete file; return -1; } // Read events and their tracks. Each data attribute is represented // by a branch in the TTree. resetAttributeVectors ( ); if (loadBranches (tree, prefix) == -1) { PRINT_ERR ("loadBranches failed!"); resetAttributeVectors ( ); delete file; return -1; } mEvents.clear ( ); // Loop over all events for (Long64_t events = 0; events < tree->GetEntriesFast ( ); ++events) { Long64_t local_events = tree->LoadTree (events); if (local_events < 0) { PRINT_ERR ("LoadTree returned value < 0"); resetAttributeVectors ( ); delete file; return -1; } // Load branches into the matching vectors if (tree->GetEntry (events) <= 0) { PRINT_ERR ("tree->GetEntry failed!"); resetAttributeVectors ( ); delete file; return -1; } if (eventSanityCheck ( ) == -1) { PRINT_ERR ("Event sanity check failed!"); resetAttributeVectors ( ); delete file; return -1; } KF_Event_t newEvent; Track_t newTrack; TrackHit_t trackHit; #if HAS_COMPETING_ROTS for (int i = 0; i < MAXROTS; ++i) { trackHit.rotIds[i] = 0; } #endif newEvent.clear(); // Loop over all tracks for (unsigned int track = 0; track < mLocX->size ( ); ++track) { newTrack.track.clear ( ); // phi, theta, qoverp are values referring to a complete track newTrack.info.d0 = (*mD0)[track]; newTrack.info.z0 = (*mZ0)[track]; newTrack.info.phi = (*mPhi)[track]; newTrack.info.theta = (*mTheta)[track]; newTrack.info.qoverp = (*mQoverP)[track]; // FIXME: has to check whether MC index is valid! //int tti = (*mTTTI)[ (*mMCI)[track] ]; // FIXME: has to check whether truth track index is valid! //std::cout << "Track:" << track << " TruthTrackIndex: " << tti <<std::endl; newTrack.truthTrackInfo.d0 = -999.; newTrack.truthTrackInfo.z0 = -999.; newTrack.truthTrackInfo.phi = -999.; newTrack.truthTrackInfo.theta = -999.; newTrack.truthTrackInfo.qoverp = -999.; int mcindex = (*mMCI)[track]; if (mcindex > -1) { if (mMCPerigeeOk->at(mcindex)) { newTrack.truthTrackInfo.d0 = (*mTTD0)[mcindex]; newTrack.truthTrackInfo.z0 = (*mTTZ0)[mcindex]; newTrack.truthTrackInfo.phi = (*mTTPhi)[mcindex]; newTrack.truthTrackInfo.theta = (*mTTTheta)[mcindex]; newTrack.truthTrackInfo.qoverp = (*mTTQoverP)[mcindex]; } } bool validTrack = true; // std::cout<< "track " << track << std::endl; // Loop over all hits for (unsigned int hit = 0; hit < (*mLocX)[track].size ( ); ++hit) { // No NULL-pointer check, since eventSanityCheck passed trackHit.normal[0] = (*mLocX)[track][hit]; trackHit.normal[1] = (*mLocY)[track][hit]; trackHit.err_locX = (*mErr_locX)[track][hit]; trackHit.err_locY = (*mErr_locY)[track][hit]; trackHit.cov_locXY = (*mCov_locXY)[track][hit]; trackHit.detType = (*mDetType)[track][hit]; trackHit.bec = (*mBec)[track][hit]; trackHit.ref[0] = (*mRefLocX)[track][hit]; trackHit.ref[1] = (*mRefLocY)[track][hit]; trackHit.ref[2] = (*mRefPhi)[track][hit]; trackHit.ref[3] = (*mRefTheta)[track][hit]; trackHit.ref[4] = (*mRefQoverP)[track][hit]; #if HAS_MATERIAL_EFFECTS trackHit.sigmaDeltaThetaSquared = pow(mSigmaDeltaTheta->at(track).at(hit), 2); trackHit.sigmaDeltaPhiSquared = pow(mSigmaDeltaPhi->at(track).at(hit), 2); //trackHit.sigmaDeltaQoverPSquared = (trackHit.ref[4]*trackHit.ref[4]>0.) ? ( mSigmaDeltaE->at(track).at(hit) * sqrt(PionMassSquared + 1./(trackHit.ref[4]*trackHit.ref[4])) * fabs(pow(trackHit.ref[4],3)) ) : 0.; const double qOverPSquared=trackHit.ref[4]*trackHit.ref[4]; trackHit.sigmaDeltaQoverPSquared = (qOverPSquared>0.) ? ( pow(mSigmaDeltaE->at(track).at(hit),2) * (PionMassSquared + 1./qOverPSquared) * pow(qOverPSquared,3) ) : 0.; #endif // #if HAS_MATERIAL_EFFECTS #if HAS_COMPETING_ROTS trackHit.competingRotIds = (*mCompetingRotIds)[track][hit]; //std::cout << "read compRot " << trackHit.competingRotIds << " track " << track << " hit " << hit << std::endl; for (unsigned int rot = 0; rot < (*mRotLocX)[track][hit].size(); ++rot) { trackHit.rotLocX[rot] = (*mRotLocX)[track][hit][rot]; trackHit.rotLocY[rot] = (*mRotLocY)[track][hit][rot]; trackHit.rotIds[rot] = (*mRotIds)[track][hit][rot]; trackHit.rotProbs[rot] = (*mRotProbs)[track][hit][rot]; trackHit.rotCov[rot][0] = (*mRotCov00)[track][hit][rot]; trackHit.rotCov[rot][1] = (*mRotCov01)[track][hit][rot]; trackHit.rotCov[rot][2] = (*mRotCov10)[track][hit][rot]; trackHit.rotCov[rot][3] = (*mRotCov11)[track][hit][rot]; // if (DBG_LVL == 0) if ((*mRotLocY)[track][hit].size() > 1) std::cout << rot << "/" << (*mRotLocY)[track][hit].size() - 1 << " id " << trackHit.rotIds[rot] << " locY " << trackHit.rotLocY[rot] << " effY " << trackHit.measurement[1] << " cov " << trackHit.rotCov[rot][1] << std::endl; // std::cout << "\trot " << trackHit.rotIds[rot] << std::endl; } #endif if (abs(trackHit.normal[1] + 99999.) > 1.E-4) { trackHit.is2Dim = 1; } else { trackHit.is2Dim = 0; } // no measurement at all (pure material surface): if (!(fabs(trackHit.normal[0] + 99999.) > 1.E-4)) { trackHit.is2Dim = 3; // std::cout<< "mat lay: sigmaDeltaPhi=" << mSigmaDeltaPhi->at(track).at(hit) // << " sigmaDeltaTheta=" << mSigmaDeltaTheta->at(track).at(hit) // << " sigmaDeltaE=" << mSigmaDeltaE->at(track).at(hit) << std::endl; }// else { // std::cout<< "meas lay: sigmaDeltaPhi=" << mSigmaDeltaPhi->at(track).at(hit) // << " sigmaDeltaTheta=" << mSigmaDeltaTheta->at(track).at(hit) // << " sigmaDeltaE=" << mSigmaDeltaE->at(track).at(hit) << std::endl; // } //Check if this track has valid Data //if (abs(trackHit.normal[0] + 999.) < 1.E-4 || abs(trackHit.normal[1] + 999.) < 1.E-4 || abs(trackHit.ref[0] + 999.) < 1.E-4 || abs(trackHit.ref[1] + 999.) < 1.E-4) { if (abs(trackHit.ref[0] + 999.) < 1.E-4 || abs(trackHit.ref[1] + 999.) < 1.E-4) { validTrack = false; break; } // Ignore Detector-Type 3 if (trackHit.detType == 3) { break; } if (setJacobiMatrix (trackHit.jacobi, track, hit) == -1) { PRINT_WARN ("Could not set JacobiMatrix for hit!"); break; } #if USE_EIGEN_MATRIX_INVERSION matrixInverseEigen(trackHit.jacobi, trackHit.jacobiInverse, ORDER, ORDER); #else matrixInverse(trackHit.jacobi, trackHit.jacobiInverse, ORDER, ORDER); #endif newTrack.track.push_back (trackHit); } // if(validTrack) // newEvent.push_back (newTrack); if (!validTrack) newTrack.track.clear ( ); newEvent.push_back (newTrack); } mEvents.push_back (newEvent); } resetAttributeVectors ( ); delete file; return 0; }