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;
}
