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