void SargassoNode::updateSpace(MDataBlock& block, unsigned idx)
{
MStatus stat;
/* // jump to elm is slow
MArrayDataHandle hparentspaces = block.inputArrayValue(aconstraintParentInverseMatrix, &stat);
// if(!stat) MGlobal::displayInfo("cannot input array");
stat = hparentspaces.jumpToElement(idx);
//if(!stat) AHelper::Info<unsigned>("cannot jump to elm", iobject);
MDataHandle hspace = hparentspaces.inputValue(&stat);
// if(!stat) MGlobal::displayInfo("cannot input single");
const MMatrix parentSpace = hspace.asMatrix();
*/
const unsigned itri = objectTriangleInd()[idx];
Matrix33F q = m_diff->Q()[itri];
q.orthoNormalize();
const Vector3F t = m_mesh->triangleCenter(itri);
Matrix44F sp(q, t);
AHelper::ConvertToMMatrix(m_currentSpace, sp);
// m_currentSpace *= parentSpace;
// AHelper::PrintMatrix("parent inv", m_currentSpace);
const Vector3F objectP = localP()[idx];
m_solvedT = MPoint(objectP.x, objectP.y, objectP.z) * m_currentSpace;
MTransformationMatrix mtm(m_currentSpace);
MTransformationMatrix::RotationOrder rotorder = MTransformationMatrix::kXYZ;
mtm.getRotation(m_rot, rotorder);
}
/// This should be subclassed to actually add some physics, based on your given game. By default it will send a PT_SET_ACCELERATION message to the entity using walkingAcceleration.
void PathableProperty::GoToWaypoint(Waypoint * wp)
{
if (wp == 0)
{
StopWalking();
return;
}
MoveToMessage mtm(wp->position);
owner->ProcessMessage(&mtm);
}
// static
void GMatrixd::homography( QVector< Vector3f > from,
QVector< Vector3f > to, GMatrixd& output )
{
output.resize( 3, 3 );
GMatrixd m( 8, 9 );
for( int i = 0; i < 4; ++i )
{
m(2*i,0) = from[i].x() * to[i].z();
m(2*i,1) = from[i].y() * to[i].z();
m(2*i,2) = from[i].z() * to[i].z();
m(2*i,6) = -from[i].x() * to[i].x();
m(2*i,7) = -from[i].y() * to[i].x();
m(2*i,8) = -from[i].z() * to[i].x();
m(2*i+1,3) = from[i].x() * to[i].z();
m(2*i+1,4) = from[i].y() * to[i].z();
m(2*i+1,5) = from[i].z() * to[i].z();
m(2*i+1,6) = -from[i].x() * to[i].y();
m(2*i+1,7) = -from[i].y() * to[i].y();
m(2*i+1,8) = -from[i].z() * to[i].y();
}
QVector< QVector< double > > eigenvectors;
QVector< double > eigenvalues;
GMatrixd mt( 9, 8 );
m.transpose( mt );
GMatrixd mtm( 9, 9 );
GMatrixd::times( mt, m, mtm );
mtm.eigenvalueDecomposition( &eigenvectors, &eigenvalues );
for( int i=0;i<3;i++){
for( int j=0;j<3;j++){
output( i, j ) = eigenvectors[0][3*i+j];
}
}
}
ModelTracker::Transform ModelTracker::epnp(const ModelTracker::ImgPoint imagePoints[])
{
/*********************************************************************
Step 1: Calculate four control points enveloping the model points by
running Principal Component Analysis on the set of model points.
*********************************************************************/
Point cps[4]; // The four control points
Geometry::Matrix<Scalar,3,3> cpm; // The matrix to calculate barycentric control point weights for model points
Geometry::PCACalculator<3> pca;
for(unsigned int mpi=0;mpi<numModelPoints;++mpi)
pca.accumulatePoint(modelPoints[mpi]);
/* First control point is model point set's centroid: */
cps[0]=Point(pca.calcCentroid());
/* Next three control points are aligned with the model point set's principal axes: */
pca.calcCovariance();
double pcaEvals[3];
pca.calcEigenvalues(pcaEvals);
Vector pcaEvecs[3];
for(int i=0;i<3;++i)
pcaEvecs[i]=Vector(pca.calcEigenvector(pcaEvals[i]));
if((pcaEvecs[0]^pcaEvecs[1])*pcaEvecs[2]<0.0)
{
// std::cout<<"World control points are left-handed!"<<std::endl;
pcaEvecs[2]*=Scalar(-1);
}
for(int i=0;i<3;++i)
for(int j=0;j<3;++j)
cpm(i,j)=pcaEvecs[i][j];
Transform worldToModel(Vector(cpm*(Point::origin-cps[0])),Transform::Rotation::fromMatrix(cpm));
for(int i=0;i<3;++i)
{
Scalar scale=Scalar(Math::sqrt(pcaEvals[i])); // Scale principal components to the model
cps[1+i]=cps[0]+pcaEvecs[i]*scale; // Should add a check for zero Eigenvalue here
/* Calculate the inverse control point matrix directly, as it's orthogonal: */
for(int j=0;j<3;++j)
cpm(i,j)/=scale;
}
#if EPNP_DEBUG
std::cout<<"Principal components: "<<Math::sqrt(pcaEvals[0])<<", "<<Math::sqrt(pcaEvals[1])<<", "<<Math::sqrt(pcaEvals[2])<<std::endl;
std::cout<<"Control points: "<<cps[0]<<", "<<cps[1]<<", "<<cps[2]<<", "<<cps[3]<<std::endl;
#endif
/*********************************************************************
Step 2: Calculate the linear system M^T*M.
*********************************************************************/
Math::Matrix mtm(12,12,0.0);
const Projection::Matrix& pm=projection.getMatrix();
Scalar fu=pm(0,0);
Scalar sk=pm(0,1);
Scalar uc=pm(0,2);
Scalar fv=pm(1,1);
Scalar vc=pm(1,2);
for(unsigned int mpi=0;mpi<numModelPoints;++mpi)
{
/* Calculate the model point's control point weights: */
Vector mpc=modelPoints[mpi]-cps[0];
Scalar alphai[4];
for(int i=0;i<3;++i)
alphai[1+i]=cpm(i,0)*mpc[0]+cpm(i,1)*mpc[1]+cpm(i,2)*mpc[2];
alphai[0]=Scalar(1)-alphai[1]-alphai[2]-alphai[3];
/* Calculate the coefficients of the model point / image point association's two linear equations: */
double eqs[2][12];
for(int i=0;i<4;++i)
{
/* Equation for image point's u coordinate: */
eqs[0][i*3+0]=alphai[i]*fu;
eqs[0][i*3+1]=alphai[i]*sk;
eqs[0][i*3+2]=alphai[i]*(uc-imagePoints[mpi][0]);
/* Equation for image point's v coordinate: */
eqs[1][i*3+0]=0.0;
eqs[1][i*3+1]=alphai[i]*fv;
eqs[1][i*3+2]=alphai[i]*(vc-imagePoints[mpi][1]);
}
/* Enter the model point / image point association's two linear equations into the least-squares matrix: */
for(unsigned int i=0;i<12;++i)
for(unsigned int j=0;j<12;++j)
mtm(i,j)+=eqs[0][i]*eqs[0][j]+eqs[1][i]*eqs[1][j];
}
/*********************************************************************
Step 3: Find four potential solutions to the pose estimation problem
by assuming that either 1, 2, 3, or 4 Eigenvalues of the least-squares
linear system are zero or very small, and calculate a scale-preserving
transformation for all cases. Then pick the one that minimizes
reprojection error.
*********************************************************************/
/* Get the full set of eigenvalues and eigenvectors of the least-squares matrix: */
std::pair<Math::Matrix,Math::Matrix> qe=mtm.jacobiIteration();
/* Find the indices of the four smallest Eigenvalues: */
unsigned int evIndices[12];
for(unsigned int i=0;i<12;++i)
evIndices[i]=i;
for(unsigned int i=0;i<4;++i)
{
/* Find the next-smallest Eigenvalue: */
int minI=i;
double minE=Math::abs(qe.second(evIndices[i],0));
for(unsigned int j=i+1;j<12;++j)
{
double e=Math::abs(qe.second(evIndices[j],0));
if(minE>e)
{
minI=j;
minE=e;
}
}
/* Move the found Eigenvalue to the front: */
int ti=evIndices[i];
evIndices[i]=evIndices[minI];
evIndices[minI]=ti;
}
#if EPNP_DEBUG
std::cout<<"MTM Eigenvalues:";
for(unsigned int i=0;i<12;++i)
std::cout<<' '<<qe.second(evIndices[i],0);
std::cout<<std::endl;
#endif
/* Calculate the pairwise distances between the four control points in world space: */
Scalar cpDists[6];
cpDists[0]=Geometry::sqrDist(cps[0],cps[1]);
cpDists[1]=Geometry::sqrDist(cps[0],cps[2]);
cpDists[2]=Geometry::sqrDist(cps[0],cps[3]);
cpDists[3]=Geometry::sqrDist(cps[1],cps[2]);
cpDists[4]=Geometry::sqrDist(cps[1],cps[3]);
cpDists[5]=Geometry::sqrDist(cps[2],cps[3]);
/*********************************************************************
Step 3a: Calculate the solution vector for the assumed case of one
very small Eigenvalue:
*********************************************************************/
/* Extract the positions of the four control points in camera space from the smallest Eigenvector: */
Point cpcs[4];
for(unsigned int cpi=0;cpi<4;++cpi)
for(unsigned int i=0;i<3;++i)
cpcs[cpi][i]=Scalar(qe.first(cpi*3+i,evIndices[0]));
/* Calculate the pairwise distances between the four control points in camera space: */
Scalar cpcDists[6];
cpcDists[0]=Geometry::sqrDist(cpcs[0],cpcs[1]);
cpcDists[1]=Geometry::sqrDist(cpcs[0],cpcs[2]);
cpcDists[2]=Geometry::sqrDist(cpcs[0],cpcs[3]);
cpcDists[3]=Geometry::sqrDist(cpcs[1],cpcs[2]);
cpcDists[4]=Geometry::sqrDist(cpcs[1],cpcs[3]);
cpcDists[5]=Geometry::sqrDist(cpcs[2],cpcs[3]);
/* Calculate the scaling factor: */
Scalar betaCounter(0);
Scalar betaDenominator(0);
for(int i=0;i<6;++i)
{
betaCounter+=Math::sqrt(cpcDists[i]*cpDists[i]);
betaDenominator+=cpcDists[i];
}
Scalar beta=-betaCounter/betaDenominator;
#if EPNP_DEBUG
std::cout<<"Scaling factor: "<<beta<<std::endl;
#endif
/* Recalculate the camera-space control points: */
for(int i=0;i<4;++i)
for(int j=0;j<3;++j)
cpcs[i][j]*=beta;
/* Check if the control point order was flipped: */
if(((cpcs[1]-cpcs[0])^(cpcs[2]-cpcs[0]))*(cpcs[3]-cpcs[0])<0.0)
{
// std::cout<<"Control points got done flipped"<<std::endl;
// cpcs[3]=cpcs[0]-(cpcs[3]-cpcs[0]);
}
#if EPNP_DEBUG
/* Print the camera-space control points: */
for(int i=0;i<4;++i)
std::cout<<"CCP "<<i<<": "<<cpcs[i][0]<<", "<<cpcs[i][1]<<", "<<cpcs[i][2]<<std::endl;
std::cout<<Math::sqrt((cpcs[1]-cpcs[0])*(cpcs[1]-cpcs[0]));
std::cout<<' '<<(cpcs[1]-cpcs[0])*(cpcs[2]-cpcs[0]);
std::cout<<' '<<(cpcs[1]-cpcs[0])*(cpcs[3]-cpcs[0]);
std::cout<<' '<<Math::sqrt((cpcs[2]-cpcs[0])*(cpcs[2]-cpcs[0]));
std::cout<<' '<<(cpcs[2]-cpcs[0])*(cpcs[3]-cpcs[0]);
std::cout<<' '<<Math::sqrt((cpcs[3]-cpcs[0])*(cpcs[3]-cpcs[0]))<<std::endl;
std::cout<<Math::sqrt((cps[1]-cps[0])*(cps[1]-cps[0]));
std::cout<<' '<<(cps[1]-cps[0])*(cps[2]-cps[0]);
std::cout<<' '<<(cps[1]-cps[0])*(cps[3]-cps[0]);
std::cout<<' '<<Math::sqrt((cps[2]-cps[0])*(cps[2]-cps[0]));
std::cout<<' '<<(cps[2]-cps[0])*(cps[3]-cps[0]);
std::cout<<' '<<Math::sqrt((cps[3]-cps[0])*(cps[3]-cps[0]))<<std::endl;
#endif
/* Calculate the transformation from camera control point space to camera space: */
Vector cbase[3];
for(int i=0;i<3;++i)
cbase[i]=cpcs[1+i]-cpcs[0];
cbase[0].normalize();
cbase[1]-=(cbase[0]*cbase[1])*cbase[0];
cbase[1].normalize();
cbase[2]-=(cbase[0]*cbase[2])*cbase[0];
cbase[2]-=(cbase[1]*cbase[2])*cbase[1];
cbase[2].normalize();
Geometry::Matrix<Scalar,3,3> camera;
for(int i=0;i<3;++i)
for(int j=0;j<3;++j)
camera(i,j)=cbase[j][i];
Transform modelToCamera(cpcs[0]-Point::origin,Transform::Rotation::fromMatrix(camera));
return modelToCamera*worldToModel;
}
