void RpalParser::Ta() { pushProc("Ta()"); Tc(); while (_nt == "aug") { read_token("aug"); Tc(); build("aug", 2); } popProc("Ta()"); }
void RpalParser::Tc() { pushProc("Tc()"); B(); if (_nt == "->") { read_token("->"); Tc(); read_token("|"); Tc(); build("->", 3); } popProc("Tc()"); }
void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3f& R, const Vec3f& T, BVHFrontList* front_list) { bool l1 = node->isFirstNodeLeaf(b1); bool l2 = node->isSecondNodeLeaf(b2); if(l1 && l2) { updateFrontList(front_list, b1, b2); if(node->BVTesting(b1, b2, R, T)) return; node->leafTesting(b1, b2, R, T); return; } if(node->BVTesting(b1, b2, R, T)) { updateFrontList(front_list, b1, b2); return; } Vec3f temp; if(node->firstOverSecond(b1, b2)) { int c1 = node->getFirstLeftChild(b1); int c2 = node->getFirstRightChild(b1); const OBB& bv1 = node->model1->getBV(c1).bv; Matrix3f Rc(R.transposeTimes(bv1.axis[0]), R.transposeTimes(bv1.axis[1]), R.transposeTimes(bv1.axis[2])); temp = T - bv1.To; Vec3f Tc(temp.dot(bv1.axis[0]), temp.dot(bv1.axis[1]), temp.dot(bv1.axis[2])); collisionRecurse(node, c1, b2, Rc, Tc, front_list); // early stop is disabled is front_list is used if(node->canStop() && !front_list) return; const OBB& bv2 = node->model1->getBV(c2).bv; Rc = Matrix3f(R.transposeTimes(bv2.axis[0]), R.transposeTimes(bv2.axis[1]), R.transposeTimes(bv2.axis[2])); temp = T - bv2.To; Tc.setValue(temp.dot(bv2.axis[0]), temp.dot(bv2.axis[1]), temp.dot(bv2.axis[2])); collisionRecurse(node, c2, b2, Rc, Tc, front_list); } else { int c1 = node->getSecondLeftChild(b2); int c2 = node->getSecondRightChild(b2); const OBB& bv1 = node->model2->getBV(c1).bv; Matrix3f Rc; temp = R * bv1.axis[0]; Rc(0, 0) = temp[0]; Rc(1, 0) = temp[1]; Rc(2, 0) = temp[2]; temp = R * bv1.axis[1]; Rc(0, 1) = temp[0]; Rc(1, 1) = temp[1]; Rc(2, 1) = temp[2]; temp = R * bv1.axis[2]; Rc(0, 2) = temp[0]; Rc(1, 2) = temp[1]; Rc(2, 2) = temp[2]; Vec3f Tc = R * bv1.To + T; collisionRecurse(node, b1, c1, Rc, Tc, front_list); // early stop is disabled is front_list is used if(node->canStop() && !front_list) return; const OBB& bv2 = node->model2->getBV(c2).bv; temp = R * bv2.axis[0]; Rc(0, 0) = temp[0]; Rc(1, 0) = temp[1]; Rc(2, 0) = temp[2]; temp = R * bv2.axis[1]; Rc(0, 1) = temp[0]; Rc(1, 1) = temp[1]; Rc(2, 1) = temp[2]; temp = R * bv2.axis[2]; Rc(0, 2) = temp[0]; Rc(1, 2) = temp[1]; Rc(2, 2) = temp[2]; Tc = R * bv2.To + T; collisionRecurse(node, b1, c2, Rc, Tc, front_list); } }
void turbulentTemperatureRadCoupledMixedFvPatchScalarField::updateCoeffs() { if (updated()) { return; } // Since we're inside initEvaluate/evaluate there might be processor // comms underway. Change the tag we use. int oldTag = UPstream::msgType(); UPstream::msgType() = oldTag+1; // Get the coupling information from the mappedPatchBase const mappedPatchBase& mpp = refCast<const mappedPatchBase>(patch().patch()); const polyMesh& nbrMesh = mpp.sampleMesh(); const label samplePatchi = mpp.samplePolyPatch().index(); const fvPatch& nbrPatch = refCast<const fvMesh>(nbrMesh).boundary()[samplePatchi]; scalarField Tc(patchInternalField()); scalarField& Tp = *this; const turbulentTemperatureRadCoupledMixedFvPatchScalarField& nbrField = refCast <const turbulentTemperatureRadCoupledMixedFvPatchScalarField> ( nbrPatch.lookupPatchField<volScalarField, scalar>(TnbrName_) ); // Swap to obtain full local values of neighbour internal field scalarField TcNbr(nbrField.patchInternalField()); mpp.distribute(TcNbr); // Swap to obtain full local values of neighbour K*delta scalarField KDeltaNbr; if (contactRes_ == 0.0) { KDeltaNbr = nbrField.kappa(nbrField)*nbrPatch.deltaCoeffs(); } else { KDeltaNbr.setSize(nbrField.size(), contactRes_); } mpp.distribute(KDeltaNbr); scalarField KDelta(kappa(Tp)*patch().deltaCoeffs()); scalarField Qr(Tp.size(), 0.0); if (QrName_ != "none") { Qr = patch().lookupPatchField<volScalarField, scalar>(QrName_); } scalarField QrNbr(Tp.size(), 0.0); if (QrNbrName_ != "none") { QrNbr = nbrPatch.lookupPatchField<volScalarField, scalar>(QrNbrName_); mpp.distribute(QrNbr); } valueFraction() = KDeltaNbr/(KDeltaNbr + KDelta); refValue() = TcNbr; refGrad() = (Qr + QrNbr)/kappa(Tp); mixedFvPatchScalarField::updateCoeffs(); if (debug) { scalar Q = gSum(kappa(Tp)*patch().magSf()*snGrad()); Info<< patch().boundaryMesh().mesh().name() << ':' << patch().name() << ':' << this->internalField().name() << " <- " << nbrMesh.name() << ':' << nbrPatch.name() << ':' << this->internalField().name() << " :" << " heat transfer rate:" << Q << " walltemperature " << " min:" << gMin(Tp) << " max:" << gMax(Tp) << " avg:" << gAverage(Tp) << endl; } // Restore tag UPstream::msgType() = oldTag; }
void Mesh::clean() { uint i, j, idist=0; Vector a, b, c, m; double mdist=0.; arr Tc(T.d0, 3); //tri centers arr Tn(T.d0, 3); //tri normals uintA Vt(V.d0); intA VT(V.d0, 100); //tri-neighbors to a vertex Vt.setZero(); VT=-1; for(i=0; i<T.d0; i++) { a.set(&V(T(i, 0), 0)); b.set(&V(T(i, 1), 0)); c.set(&V(T(i, 2), 0)); //tri center m=(a+b+c)/3.; Tc(i, 0)=m.x; Tc(i, 1)=m.y; Tc(i, 2)=m.z; //farthest tri if(m.length()>mdist) { mdist=m.length(); idist=i; } //tri normal b-=a; c-=a; a=b^c; a.normalize(); Tn(i, 0)=a.x; Tn(i, 1)=a.y; Tn(i, 2)=a.z; //vertex neighbor count j=T(i, 0); VT(j, Vt(j))=i; Vt(j)++; j=T(i, 1); VT(j, Vt(j))=i; Vt(j)++; j=T(i, 2); VT(j, Vt(j))=i; Vt(j)++; } //step through tri list and flip them if necessary boolA Tisok(T.d0); Tisok=false; uintA Tok; //contains the list of all tris that are ok oriented uintA Tnew(T.d0, T.d1); Tok.append(idist); Tisok(idist)=true; int A=0, B=0, D; uint r, k, l; intA neighbors; for(k=0; k<Tok.N; k++) { i=Tok(k); Tnew(k, 0)=T(i, 0); Tnew(k, 1)=T(i, 1); Tnew(k, 2)=T(i, 2); for(r=0; r<3; r++) { if(r==0) { A=T(i, 0); B=T(i, 1); /*C=T(i, 2);*/ } if(r==1) { A=T(i, 1); B=T(i, 2); /*C=T(i, 0);*/ } if(r==2) { A=T(i, 2); B=T(i, 0); /*C=T(i, 1);*/ } //check all triangles that share A & B setSection(neighbors, VT[A], VT[B]); neighbors.removeAllValues(-1); if(neighbors.N>2) MT_MSG("edge shared by more than 2 triangles " <<neighbors); neighbors.removeValue(i); //if(!neighbors.N) cout <<"mesh.clean warning: edge has only one triangle that shares it" <<endl; //orient them correctly for(l=0; l<neighbors.N; l++) { j=neighbors(l); //j is a neighboring triangle sharing A & B D=-1; //align the neighboring triangle and let D be its 3rd vertex if((int)T(j, 0)==A && (int)T(j, 1)==B) D=T(j, 2); if((int)T(j, 0)==A && (int)T(j, 2)==B) D=T(j, 1); if((int)T(j, 1)==A && (int)T(j, 2)==B) D=T(j, 0); if((int)T(j, 1)==A && (int)T(j, 0)==B) D=T(j, 2); if((int)T(j, 2)==A && (int)T(j, 0)==B) D=T(j, 1); if((int)T(j, 2)==A && (int)T(j, 1)==B) D=T(j, 0); if(D==-1) HALT("dammit"); //determine orientation if(!Tisok(j)) { T(j, 0)=B; T(j, 1)=A; T(j, 2)=D; Tok.append(j); Tisok(j)=true; } else { //check if consistent! } } #if 0 //compute their rotation if(neighbors.N>1) { double phi, phimax; int jmax=-1; Vector ni, nj; for(l=0; l<neighbors.N; l++) { j=neighbors(l); //j is a neighboring triangle sharing A & B a.set(&V(T(i, 0), 0)); b.set(&V(T(i, 1), 0)); c.set(&V(T(i, 2), 0)); b-=a; c-=a; a=b^c; a.normalize(); ni = a; a.set(&V(T(j, 0), 0)); b.set(&V(T(j, 1), 0)); c.set(&V(T(j, 2), 0)); b-=a; c-=a; a=b^c; a.normalize(); nj = a; Quaternion q; q.setDiff(ni, -nj); q.getDeg(phi, c); a.set(&V(A, 0)); b.set(&V(B, 0)); if(c*(a-b) < 0.) phi+=180.; if(jmax==-1 || phi>phimax) { jmax=j; phimax=phi; } } if(!Tisok(jmax)) { Tok.append(jmax); Tisok(jmax)=true; } } else { j = neighbors(0); if(!Tisok(j)) { Tok.append(j); Tisok(j)=true; } } #endif } } if(k<T.d0) { cout <<"mesh.clean warning: not all triangles connected: " <<k <<"<" <<T.d0 <<endl; cout <<"WARNING: cutting of all non-connected triangles!!" <<endl; Tnew.resizeCopy(k, 3); T=Tnew; deleteUnusedVertices(); } computeNormals(); }
void Calib::computeProjectionMatrix() { std::cout << "1111111111***********************************************************************************\n"; // for(int i = 0;i < imagePoints.size();i++) // { // std::cout<<" imagePoints[i]= "<< imagePoints[i]<<std::endl; // std::cout<<" objectPoints[i]= "<< objectPoints[i]<<std::endl; // } if (objectPoints.size() != imagePoints.size()) return; int n = objectPoints.size(); Eigen::Vector2d avg2; Eigen::Vector3d avg3; for (unsigned int i = 0; i < n; i++) { avg2 += imagePoints[i]; avg3 += objectPoints[i]; std::cout << i << " " << objectPoints[i](0) << " " << objectPoints[i](1) << " " << objectPoints[i](2) << std::endl; } avg2 = avg2 / n; avg3 = avg3 / n; std::cout << "avg2 = " << avg2 << std::endl; std::cout << "avg3 = " << avg3 << std::endl; /* ******************************************************************************* * Data normalization * Translates and normalises a set of 2D homogeneous points so that their centroid is at the origin and their mean distance from the origin is sqrt(2). */ float meandist2 = 0; float meandist3 = 0; imagePoints_t.resize(n); objectPoints_t.resize(n); for (unsigned int i = 0; i < n; i++) { imagePoints_t[i] = imagePoints[i] - avg2; objectPoints_t[i] = objectPoints[i] - avg3; // std::cout << "1 imagePoints_t[i] = " << imagePoints_t[i] << std::endl; // std::cout << "1 objectPoints_t[i] = " << objectPoints_t[i] << std::endl; meandist2 += sqrt(imagePoints_t[i](0) * imagePoints_t[i](0) + imagePoints_t[i](1) * imagePoints_t[i](1)); meandist3 += sqrt(objectPoints_t[i](0) * objectPoints_t[i](0) + objectPoints_t[i](1) * objectPoints_t[i](1) + objectPoints_t[i](2) * objectPoints_t[i](2)); } meandist2 /= n; meandist3 /= n; std::cout << "meandist2 = " << meandist2 << std::endl; std::cout << "meandist3 = " << meandist3 << std::endl; float scale2 = sqrt(2) / meandist2; float scale3 = sqrt(3) / meandist3; std::cout << "2222222222222***********************************************************************************\n"; for (unsigned int i = 0; i < n; i++) { imagePoints_t[i] = scale2 * imagePoints_t[i]; objectPoints_t[i] = scale3 * objectPoints_t[i]; // std::cout << "imagePoints_t[i] = " << imagePoints_t[i] << std::endl; // std::cout << "objectPoints_t[i] = " << objectPoints_t[i] << std::endl; } // std::cout<<avg3<<std::endl; /* ******************************************************************************* * Similarity transformation T1 to normalize the image points, * and a second similarity transformation T2 to normalize the space points. * Page 181 in Multiple_View_Geometry_in_Computer_Vision__2nd_Edition */ Eigen::Matrix3d T1; T1 << scale2, 0, -scale2 * avg2(0), 0, scale2, -scale2 * avg2(1), 0, 0, 1; Eigen::MatrixXd T2(4, 4); T2 << scale3, 0, 0, -scale3 * avg3(0), 0, scale3, 0, -scale3 * avg3(1), 0, 0, scale3, -scale3 * avg3(2), 0, 0, 0, 1; // use Eigen Eigen::MatrixXd A(2 * n, 12); A.setZero(2 * n, 12); for (int i = 0; i < n; i++) { A(2 * i, 0) = objectPoints_t[i](0); A(2 * i, 1) = objectPoints_t[i](1); A(2 * i, 2) = objectPoints_t[i](2); A(2 * i, 3) = 1; A(2 * i, 4) = 0; A(2 * i, 5) = 0; A(2 * i, 6) = 0; A(2 * i, 7) = 0; A(2 * i, 8) = -imagePoints_t[i](0) * objectPoints_t[i](0); A(2 * i, 9) = -imagePoints_t[i](0) * objectPoints_t[i](1); A(2 * i, 10) = -imagePoints_t[i](0) * objectPoints_t[i](2); A(2 * i, 11) = -imagePoints_t[i](0) * 1; A(2 * i + 1, 0) = 0; A(2 * i + 1, 1) = 0; A(2 * i + 1, 2) = 0; A(2 * i + 1, 3) = 0; A(2 * i + 1, 4) = 1 * objectPoints_t[i](0); A(2 * i + 1, 5) = 1 * objectPoints_t[i](1); A(2 * i + 1, 6) = 1 * objectPoints_t[i](2); A(2 * i + 1, 7) = 1; A(2 * i + 1, 8) = -imagePoints_t[i](1) * objectPoints_t[i](0); A(2 * i + 1, 9) = -imagePoints_t[i](1) * objectPoints_t[i](1); A(2 * i + 1, 10) = -imagePoints_t[i](1) * objectPoints_t[i](2); A(2 * i + 1, 11) = -imagePoints_t[i](1) * 1; } Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); const Eigen::VectorXd & v1 = svd.matrixV().col(11); this->Pt << v1(0), v1(1), v1(2), v1(3), v1(4), v1(5), v1(6), v1(7), v1(8), v1(9), v1(10), v1(11); // std::cout<<"A= \n"<< A<<std::endl; // std::cout<<Pt<<std::endl; P = T1.inverse() * Pt * T2; //Decompose the projection matrix cv::Mat Pr(3, 4, cv::DataType<float>::type); cv::Mat Mt(3, 3, cv::DataType<float>::type); cv::Mat Rt(3, 3, cv::DataType<float>::type); cv::Mat Tt(4, 1, cv::DataType<float>::type); for (unsigned i = 0; i < 3; i++) { for (unsigned int j = 0; j < 4; j++) { Pr.at<float> (i, j) = P(i, j); } } cv::decomposeProjectionMatrix(Pr, Mt, Rt, Tt); //scale: Mt(2,2) should = 1; so update the projection matrix and decomposition again float k = (1 / (Mt.at<float> (2, 2))); /* **************************************************************************************** * Upate the projection matrix * Decomposition again to get new intrinsic matrix and rotation matrix */ this->P = k * P; cv::Mat Pro(3, 4, cv::DataType<float>::type); cv::Mat Mc(3, 3, cv::DataType<float>::type); // intrinsic parameter matrix cv::Mat Rc(3, 3, cv::DataType<float>::type); // rotation matrix cv::Mat Tc(4, 1, cv::DataType<float>::type); // translation vector for (unsigned i = 0; i < 3; i++) { for (unsigned int j = 0; j < 4; j++) { Pro.at<float> (i, j) = P(i, j); } } cv::decomposeProjectionMatrix(Pro, Mc, Rc, Tc); // Change from OpenCV varibles to Eigen for (unsigned i = 0; i < 3; i++) { for (unsigned int j = 0; j < 3; j++) { M(i, j) = Mc.at<float> (i, j) ; } } /* **************************************************************************************** * Compute te rotation matrix R and translation vector T */ P_temp = M.inverse() * P; this->R = this->P_temp.block(0,0,3,3); this->T = this->P_temp.block(0,3,3,1); std::cout << "+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; // std::cout << "T1 =\n " << T2 << std::endl; // std::cout << "T2 =\n " << T1 << std::endl; // std::cout << "A =\n " << A << std::endl; // std::cout << "svd.matrixV() =\n " << svd.matrixV() << std::endl; // std::cout << "Pt =\n " << Pt << std::endl; std::cout << "P =\n " << P << std::endl; std::cout << "M =\n " << M << std::endl; std::cout << "R =\n " << R << std::endl; std::cout << "Rc =\n " << Rc << std::endl; std::cout << "T =\n " << T << std::endl; std::cout << "Mt(2,2) = " << Mt.at<float> (2, 2) << std::endl; }