void X_function_pre (double theta, double Psi, double tPsiA, double tPsiB, double PsiA, double PsiB, double B, double Bm, complex<double> N[], complex<double> Nm[], const double ko)
{
double D, D1, D2;
//
complex<double> a = Iunit * ko * B;
complex<double> am = Iunit * ko * Bm;
//
if (Psi >= PsiB && Psi >= PsiA)
{
D = sqrt(3.0) / sin(Psi);
X2(D, a, N);
X2 (D, am, Nm);
}
else if (theta >= M_PI_2)
{
D = sqrt(3.0) / (cos(Psi) * tPsiA);
X2 (D, a, N);
X2 (D, am, Nm);
}
else if (Psi >= PsiA)
{
D1 = 2.0 * sqrt(3.0) / (cos(Psi) * ( tPsiB + tan(Psi) ) );
D2 = sin(Psi) / sqrt(3.0);
X1(Psi, D1, D2, tPsiB, a, N);
X1(Psi, D1, D2, tPsiB, am, Nm);
}
else
{
D1 = sqrt(3.0) / (cos(Psi) * sin(theta) );
D2 = ( cos(Psi) * tPsiA ) / sqrt(3.0);
X1(Psi, D1, D2, tPsiB, a, N);
X1(Psi, D1, D2, tPsiB, am, Nm);
}
}
Dll * loadDll( const char *moduleName )
{
E_ASSERT( moduleName );
char binPath[300];
sprintf( binPath, "%s%cbin", e6Path, sys::fileSeparator() );
const char * oldPath = sys::getCurrentDir();
sys::setCurrentDir( binPath );
$X1("load (" << moduleName << ")" );
Dll * dll = new Dll;
if ( ! dll->open( moduleName ) )
{
delete dll;
sys::setCurrentDir( oldPath );
return 0;
}
strcpy( dll->moduleName, moduleName );
mods.insert( std::make_pair( dll->moduleName, dll ) );
// reset
sys::setCurrentDir( oldPath );
return dll;
}
/// \brief Update Kalman filter
void UnscentedKalmanFilter::updateUKF(vnl_vector<double> u, vnl_vector<double> z)
{
// create Sigma points X for the current estimate distribution
vnl_matrix<double> X(N,2*N+1);
sigmas(x_hat, P_hat+Q, sqrt(C), X);
vnl_matrix<double> Dbg;
qDebug() << "X: ";
for( int i = 0; i<6; i++)
{
qDebug() << X(i,0) << " " << X(i,1) << " " << X(i,2) << " " << X(i,3) << " " << X(i,4) << " " << X(i,5) << " "
<< X(i,6) << " " << X(i,7) << " " << X(i,8) << " " << X(i,9) << " " << X(i,10) << " " << X(i,11)
<< " " << X(i,12);
}
// apply unscented transformation for process model
vnl_vector<double> x1(6);
vnl_matrix<double> X1(N,2*N+1), P1(N,N), X2(N,2*N+1);
utf(X, u, x1, X1, P1, X2);
// apply unscented transformation for observation model
vnl_vector<double> z1(M);
vnl_matrix<double> Z1(M,2*M+1), P2(M,M), Z2(M,2*M+1);
utg(X1, z1, Z1, P2, Z2);
// define transformated cross-covariance
vnl_matrix<double> WC(2*N+1,2*N+1,0.0);
WC.set_diagonal(Wc.get_row(0));
vnl_matrix<double> P12 = X2*WC*Z2.transpose();
// perform state update
K = P12*vnl_matrix_inverse<double>(P2);
x_hat = x1 + K*(z-z1);
// perform covariance update
P_hat = P1 - K*P12.transpose();
}
void test() {
g1(X1());
g2(X2()); // expected-warning{{C++98 requires an accessible copy constructor for class 'X2' when binding a reference to a temporary; was private}}
g3(X3()); // expected-warning{{no viable constructor copying parameter of type 'X3'}}
g4(X4<int>());
g5(X5()); // Generates a warning in the default argument.
}
template <class cCRNode> void cTplCoxRoyAlgo<cCRNode>::SetStdCostRegul(double aCoeff,double aCste,int aVmin)
{
for (int anX=X0(); anX<X1() ; anX++)
for (int anY=Y0(); anY<Y1() ; anY++)
for (int aZ = ZMin(anX,anY); aZ< ZMax(anX,anY) ; aZ++)
{
cRoyPt aP1 (anX,anY,aZ);
cCRNode & aS1 = NodeOfP(aP1);
int aC1 = aS1.ResidualFlow(mDirZPlus);
for (int anEdg=0; anEdg<NbEdges() ; anEdg++)
{
if (aS1.EdgeIsValide(anEdg) && (!tabCRIsVertical[anEdg]))
{
cRoyPt aP2(aP1,anEdg);
cCRNode & aS2 = NodeOfP(aP2);
int aC2 = aS2.ResidualFlow(mDirZPlus);
double aCost = aCste + aCoeff*(aC1+aC2)/2.0;
if (Cnx8())
aCost *= tabCRIsArcV8[anEdg] ? 0.2928 : 0.4142 ;
int iCost = int(aCost+0.5);
if (iCost<aVmin)
iCost = aVmin;
aS1.SetResidualFlow(anEdg,iCost);
}
}
}
}
virtual ~CEngine()
{
$X1("del()");
for ( ModIter it = mods.begin(); it != mods.end(); ++it )
{
const char * s = it->first;
Dll * d = it->second;
ClassInfo *info = d->getClassInfo();
char b[300];
char b1[1024];
bool alive=0;
b[0] = b1[0] = 0;
while ( info->iname )
{
if ( info->count() )
{
alive=1;
sprintf( b, "%s %i instances of %s alive !\n", info->cname, info->count(), info->iname );
strcat( b1, b );
}
++info;
}
if ( alive )
sys::alert("ref trouble !", b1 );
E_DELETE( d );
}
}
// Same as the runKernel, m1 and m2 should be
// 1 row x n col x 2 channels.
// And also, error has to be of CV_32FC1.
void CvOnePointEstimator::computeReprojError( const CvMat* m1, const CvMat* m2,
const CvMat* model, CvMat* error )
{
cv::Mat X1(m1), X2(m2);
int n = X1.cols;
X1 = X1.reshape(1, n);
X2 = X2.reshape(1, n);
X1.convertTo(X1, CV_64F);
X2.convertTo(X2, CV_64F);
double theta = model->data.db[0];
// Note this E is for image normal camera system, not the system in 1-pt paper
cv::Mat E = (cv::Mat_<double>(3, 3) << 0, -cos(theta * 0.5), 0,
cos(theta * 0.5), 0, -sin(theta * 0.5),
0, -sin(theta * 0.5), 0);
for (int i = 0; i < n; i++)
{
cv::Mat x1 = (cv::Mat_<double>(3, 1) << X1.at<double>(i, 0), X1.at<double>(i, 1), 1.0);
cv::Mat x2 = (cv::Mat_<double>(3, 1) << X2.at<double>(i, 0), X2.at<double>(i, 1), 1.0);
double x2tEx1 = x2.dot(E * x1);
cv::Mat Ex1 = E * x1;
cv::Mat Etx2 = E * x2;
double a = Ex1.at<double>(0) * Ex1.at<double>(0);
double b = Ex1.at<double>(1) * Ex1.at<double>(1);
double c = Etx2.at<double>(0) * Etx2.at<double>(0);
double d = Etx2.at<double>(0) * Etx2.at<double>(0);
error->data.fl[i] = x2tEx1 * x2tEx1 / (a + b + c + d);
}
}
void ATO::Integrator<T>::trisFromPoly(
std::vector< Vector3D >& points,
std::vector< Tri >& tris,
Teuchos::RCP<MiniPoly> poly)
/******************************************************************************/
{
std::vector<Vector3D>& polyPoints = poly->points;
uint nPoints = polyPoints.size();
if(nPoints < 3) return;
// find centerpoint
Vector3D center(polyPoints[0]);
for(uint i=1; i<nPoints; i++) center += polyPoints[i];
center /= nPoints;
// sort by counterclockwise angle about surface normal
T pi = acos(-1.0);
Vector3D X(polyPoints[0]-center);
T xnorm = Intrepid::norm(X);
X /= xnorm;
Vector3D X1(polyPoints[1]-center);
Vector3D Z = Intrepid::cross(X, X1);
T znorm = Intrepid::norm(Z);
Z /= znorm;
Vector3D Y = Intrepid::cross(Z, X);
std::map<T, uint> angles;
angles.insert( std::pair<T, uint>(0.0,0) );
for(int i=1; i<nPoints; i++){
Vector3D comp = polyPoints[i] - center;
T compnorm = Intrepid::norm(comp);
comp /= compnorm;
T prod = X*comp;
T angle = acos((float)prod);
if( Y * comp < 0.0 ) angle = 2.0*pi - angle;
angles.insert( std::pair<T, uint>(angle,i) );
}
// append points
int offset = points.size();
for(int pt=0; pt<nPoints; pt++){
points.push_back(polyPoints[pt]);
}
// create tris
if( angles.size() > 2 ){
typename std::map<T,uint>::iterator it=angles.begin();
typename std::map<T,uint>::iterator last=angles.end();
std::advance(last,-1); // skip last point
int iP = it->second; it++;
while(it != last){
int i1 = it->second; it++;
int i2 = it->second;
tris.push_back(Tri(iP+offset,i1+offset,i2+offset));
}
}
}
inline void
TrmmLLNA
( UnitOrNonUnit diag,
T alpha, const DistMatrix<T>& L,
DistMatrix<T>& X )
{
#ifndef RELEASE
PushCallStack("internal::TrmmLLNA");
if( L.Grid() != X.Grid() )
throw std::logic_error
("L and X must be distributed over the same grid");
if( L.Height() != L.Width() || L.Width() != X.Height() )
{
std::ostringstream msg;
msg << "Nonconformal TrmmLLNA: \n"
<< " L ~ " << L.Height() << " x " << L.Width() << "\n"
<< " X ~ " << X.Height() << " x " << X.Width() << "\n";
throw std::logic_error( msg.str().c_str() );
}
#endif
const Grid& g = L.Grid();
DistMatrix<T>
XL(g), XR(g),
X0(g), X1(g), X2(g);
DistMatrix<T,VR, STAR> X1_VR_STAR(g);
DistMatrix<T,STAR,MR > X1Trans_STAR_MR(g);
DistMatrix<T,MC, STAR> Z1_MC_STAR(g);
X1_VR_STAR.AlignWith( L );
X1Trans_STAR_MR.AlignWith( L );
Z1_MC_STAR.AlignWith( L );
PartitionRight( X, XL, XR, 0 );
while( XL.Width() < X.Width() )
{
RepartitionRight
( XL, /**/ XR,
X0, /**/ X1, X2 );
Zeros( X1.Height(), X1.Width(), Z1_MC_STAR );
//--------------------------------------------------------------------//
X1_VR_STAR = X1;
X1Trans_STAR_MR.TransposeFrom( X1_VR_STAR );
LocalTrmmAccumulateLLN
( TRANSPOSE, diag, alpha, L, X1Trans_STAR_MR, Z1_MC_STAR );
X1.SumScatterFrom( Z1_MC_STAR );
//--------------------------------------------------------------------//
SlidePartitionRight
( XL, /**/ XR,
X0, X1, /**/ X2 );
}
#ifndef RELEASE
PopCallStack();
#endif
}
Dll::Dll()
: cls(0)
, handle(0)
{
$X1("new()");
mv.modVersion = "";
mv.e6Version = "";
}
// this could be implemented more effectively
RFunction Rotation2D(double a)
{
double c = cos(a);
double s = sin(a);
RFunction X1(X(1,2));
RFunction X2(X(2,2));
return ((X1*c-X2*s,X1*s+X2*c));
}
inline void
TrmmRUNA
( UnitOrNonUnit diag,
T alpha, const DistMatrix<T>& U,
DistMatrix<T>& X )
{
#ifndef RELEASE
CallStackEntry entry("internal::TrmmRUNA");
if( U.Grid() != X.Grid() )
throw std::logic_error("{U,X} must be distributed over the same grid");
#endif
const Grid& g = U.Grid();
DistMatrix<T>
XT(g), X0(g),
XB(g), X1(g),
X2(g);
DistMatrix<T,STAR,VC > X1_STAR_VC(g);
DistMatrix<T,STAR,MC > X1_STAR_MC(g);
DistMatrix<T,MR, STAR> Z1Trans_MR_STAR(g);
DistMatrix<T,MR, MC > Z1Trans_MR_MC(g);
X1_STAR_VC.AlignWith( U );
X1_STAR_MC.AlignWith( U );
Z1Trans_MR_STAR.AlignWith( U );
PartitionDown
( X, XT,
XB, 0 );
while( XT.Height() < X.Height() )
{
RepartitionDown
( XT, X0,
/**/ /**/
X1,
XB, X2 );
Z1Trans_MR_MC.AlignWith( X1 );
//--------------------------------------------------------------------//
X1_STAR_VC = X1;
X1_STAR_MC = X1_STAR_VC;
Zeros( Z1Trans_MR_STAR, X1.Width(), X1.Height() );
LocalTrmmAccumulateRUN
( TRANSPOSE, diag, alpha, U, X1_STAR_MC, Z1Trans_MR_STAR );
Z1Trans_MR_MC.SumScatterFrom( Z1Trans_MR_STAR );
Transpose( Z1Trans_MR_MC.Matrix(), X1.Matrix() );
//--------------------------------------------------------------------//
Z1Trans_MR_MC.FreeAlignments();
SlidePartitionDown
( XT, X0,
X1,
/**/ /**/
XB, X2 );
}
}
uint unloadDll( const char *moduleName )
{
$X1("unload (" << moduleName << ")" );
ModIter it = mods.find( moduleName );
if ( it != mods.end() )
{
Dll* h = it->second;
E_DELETE( h );
return 1;
}
return 0;
}
dMatrix clSpline::GetFirstDerivatives(void)
{
dMatrix dummy;
if(!initialised)
{
Initialise();
}
dummy.SetNumberRows(X1.GetNumberRows());
dummy.SetNumberColumns(3);
for(int i=1; i<=X1.GetNumberRows(); i++)
{
dummy.SetElement(i, 1, fabs(X1(i, 1)) < PRECISION ? 0.0 : X1(i, 1));
dummy.SetElement(i, 2, fabs(Y1(i, 1)) < PRECISION ? 0.0 : Y1(i, 1));
dummy.SetElement(i, 3, fabs(Z1(i, 1)) < PRECISION ? 0.0 : Z1(i, 1));
}
return(dummy);
}
void vpTemplateTrackerWarp::findWarp(const double *ut0,const double *vt0,const double *u,const double *v,int nb_pt,vpColVector& p)
{
vpMatrix dW_(2,nbParam);
vpMatrix dX(2,1);
vpMatrix H(nbParam,nbParam), HLM(nbParam,nbParam);
vpMatrix G(nbParam,1);
int cpt=0;
vpColVector X1(2);
vpColVector fX1(2);
vpColVector X2(2);
double erreur=0;
double erreur_prec;
double lambda=0.01;
do
{
erreur_prec=erreur;
H=0;
G=0;
erreur=0;
computeCoeff(p);
for(int i=0;i<nb_pt;i++)
{
X1[0]=ut0[i];
X1[1]=vt0[i];
computeDenom(X1,p);
warpX(X1,fX1,p);
dWarp(X1,fX1,p,dW_);
H+=dW_.AtA();
X2[0]=u[i];
X2[1]=v[i];
dX=X2-fX1;
G+=dW_.t()*dX;
erreur+=((u[i]-fX1[0])*(u[i]-fX1[0])+(v[i]-fX1[1])*(v[i]-fX1[1]));
}
vpMatrix::computeHLM(H, lambda, HLM);
try{
p+=HLM.inverseByLU()*G;
}
catch(vpException &e) {
//std::cout<<"Cannot inverse the matrix by LU " << std::endl;
throw(e);
}
cpt++;
}
while((cpt<150)&&(sqrt((erreur_prec-erreur)*(erreur_prec-erreur))>1e-20));
//std::cout<<"erreur apres transformation="<<erreur<<std::endl;
}
void vpTemplateTrackerWarp::warp(const double *ut0,const double *vt0,int nb_pt,const vpColVector& p,double *u,double *v)
{
computeCoeff(p);
vpColVector X1(2),X2(2);
for(int i=0;i<nb_pt;i++)
{
X1[0]=ut0[i];
X1[1]=vt0[i];
computeDenom(X1,p);
warpX(X1,X2,p);
u[i]=X2[0];
v[i]=X2[1];
//std::cout<<"warp "<<X2[0]<<","<<X2[1]<<std::endl;
}
}
static
void special_assignments(void)
{
if (flag(Opt->integer_ring)) {
/* Fix [+,-,*] as the ring of integers mod domain_size. */
/* If any of those operations doesn't exist, then ignore it.*/
Symbol_data s;
for (s = Symbols; s != NULL; s = s->next) {
int i, j;
if (is_symbol(s->sn, "+", 2)) {
for (i = 0; i < Domain_size; i++)
for (j = 0; j < Domain_size; j++)
Cells[X2(s->base,i,j)].value = Domain[(i + j) % Domain_size];
}
else if (is_symbol(s->sn, "*", 2)) {
for (i = 0; i < Domain_size; i++)
for (j = 0; j < Domain_size; j++)
Cells[X2(s->base,i,j)].value = Domain[(i * j) % Domain_size];
}
else if (is_symbol(s->sn, "-", 1)) {
for (i = 0; i < Domain_size; i++)
Cells[X1(s->base,i)].value = Domain[(Domain_size - i) % Domain_size];
}
else if (is_symbol(s->sn, "--", 2)) {
for (i = 0; i < Domain_size; i++)
for (j = 0; j < Domain_size; j++)
Cells[X2(s->base,i,j)].value = Domain[((i + Domain_size) - j) %
Domain_size];
}
}
}
if (flag(Opt->order_domain)) {
Symbol_data s;
for (s = Symbols; s != NULL; s = s->next) {
int i, j;
if (is_symbol(s->sn, "<", 2)) {
for (i = 0; i < Domain_size; i++)
for (j = 0; j < Domain_size; j++)
Cells[X2(s->base,i,j)].value = (Domain[i<j ? 1 : 0]);
}
if (is_symbol(s->sn, "<=", 2)) {
for (i = 0; i < Domain_size; i++)
for (j = 0; j < Domain_size; j++)
Cells[X2(s->base,i,j)].value = (Domain[i<=j ? 1 : 0]);
}
}
}
} /* special_assignments */
double vpTemplateTrackerWarp::getDistanceBetweenZoneAndWarpedZone(const vpTemplateTrackerZone &Z, const vpColVector &p)
{
unsigned int nb_corners = Z.getNbTriangle() * 3;
computeCoeff(p);
vpColVector X1(2),X2(2);
double res=0;
vpTemplateTrackerTriangle triangle;
for(unsigned int i=0;i<Z.getNbTriangle();i++)
{
Z.getTriangle(i, triangle);
for (unsigned int j=0; j<3; j++) {
triangle.getCorner(j, X1[0], X1[1]);
computeDenom(X1,p);
warpX(X1,X2,p);
res+=sqrt((X2[0]-X1[0])*(X2[0]-X1[0])+(X2[1]-X1[1])*(X2[1]-X1[1]));
}
}
return res/nb_corners;
}
void benchmark_scan(
const vex::Context &ctx, vex::profiler<> &prof
)
{
const size_t N = 16 * 1024 * 1024;
const size_t M = 16;
typedef typename std::conditional<
std::is_same<float, real>::value, cl_uint, cl_ulong
>::type key_type;
std::default_random_engine rng( std::rand() );
std::uniform_int_distribution<key_type> rnd;
std::vector<key_type> x0(N);
std::vector<key_type> x1(N);
std::generate(x0.begin(), x0.end(), [&]() { return rnd(rng); });
vex::vector<key_type> X0(ctx, x0);
vex::vector<key_type> X1(ctx, N);
vex::exclusive_scan(X0, X1);
ctx.finish();
prof.tic_cpu("VexCL");
for(size_t i = 0; i < M; i++)
vex::exclusive_scan(X0, X1);
ctx.finish();
double tot_time = prof.toc("VexCL");
std::cout
<< "Scan (" << vex::type_name<key_type>() << ")\n"
<< " VexCL: " << N * M / tot_time << " keys/sec\n";
#ifdef HAVE_BOOST_COMPUTE
vex::compute::exclusive_scan(X0, X1);
ctx.finish();
prof.tic_cpu("Boost.Compute");
for(size_t i = 0; i < M; i++)
vex::compute::exclusive_scan(X0, X1);
ctx.finish();
tot_time = prof.toc("Boost.Compute");
std::cout
<< " Boost.Compute: " << N * M / tot_time << " keys/sec\n";
#endif
#ifdef HAVE_CLOGS
vex::clogs::exclusive_scan(X0, X1);
ctx.finish();
prof.tic_cpu("CLOGS");
for(size_t i = 0; i < M; i++)
vex::clogs::exclusive_scan(X0, X1);
ctx.finish();
tot_time = prof.toc("CLOGS");
std::cout
<< " CLOGS: " << N * M / tot_time << " keys/sec\n";
#endif
if (options.bm_cpu) {
prof.tic_cpu("CPU");
for(size_t i = 0; i < M; i++) {
key_type sum = key_type();
for(size_t j = 0; j < N; ++j) {
key_type next = sum + x0[j];
x1[j] = sum;
sum = next;
}
}
tot_time = prof.toc("CPU");
std::cout << " CPU: " << N * M / tot_time << " keys/sec\n";
}
std::cout << std::endl;
}
void benchmark_sort(
const vex::Context &ctx, vex::profiler<> &prof
)
{
const size_t N = 16 * 1024 * 1024;
const size_t M = 16;
typedef typename std::conditional<
std::is_same<float, real>::value, cl_uint, cl_ulong
>::type key_type;
std::default_random_engine rng( std::rand() );
std::uniform_int_distribution<key_type> rnd;
std::vector<key_type> x0(N);
std::vector<key_type> x1(N);
std::generate(x0.begin(), x0.end(), [&]() { return rnd(rng); });
vex::vector<key_type> X0(ctx, x0);
vex::vector<key_type> X1(ctx, N);
X1 = X0;
vex::sort(X1);
double tot_time = 0;
for(size_t i = 0; i < M; i++) {
X1 = X0;
ctx.finish();
prof.tic_cpu("VexCL");
vex::sort(X1);
ctx.finish();
tot_time += prof.toc("VexCL");
}
std::cout
<< "Sort (" << vex::type_name<key_type>() << ")\n"
<< " VexCL: " << N * M / tot_time << " keys/sec\n";
#ifdef HAVE_BOOST_COMPUTE
X1 = X0;
vex::compute::sort(X1);
tot_time = 0;
for(size_t i = 0; i < M; i++) {
X1 = X0;
ctx.finish();
prof.tic_cpu("Boost.Compute");
vex::compute::sort(X1);
ctx.finish();
tot_time += prof.toc("Boost.Compute");
}
std::cout
<< " Boost.Compute: " << N * M / tot_time << " keys/sec\n";
#endif
#ifdef HAVE_CLOGS
X1 = X0;
vex::clogs::sort(X1);
tot_time = 0;
for(size_t i = 0; i < M; i++) {
X1 = X0;
ctx.finish();
prof.tic_cpu("CLOGS");
vex::clogs::sort(X1);
ctx.finish();
tot_time += prof.toc("CLOGS");
}
std::cout
<< " CLOGS: " << N * M / tot_time << " keys/sec\n";
#endif
if (options.bm_cpu) {
tot_time = 0;
for(size_t i = 0; i < M; i++) {
std::copy(x0.begin(), x0.end(), x1.begin());
prof.tic_cpu("STL");
std::sort(x1.begin(), x1.end());
tot_time += prof.toc("STL");
}
std::cout << " STL: " << N * M / tot_time << " keys/sec\n";
}
std::cout << std::endl;
}
void dooptab()
{ int i;
FILE *f;
/* Load optab[] */
#define X1(arr,mask) for(i=0;i<sizeof(arr)/sizeof(int);i++)xptab1[arr[i]]|=mask;
#define X2(arr,mask) for(i=0;i<sizeof(arr)/sizeof(int);i++)xptab2[arr[i]]|=mask;
#define X3(arr,mask) for(i=0;i<sizeof(arr)/sizeof(int);i++)xptab3[arr[i]]|=mask;
X1(_binary,_OTbinary);
X1(_unary,_OTunary);
X1(_commut,_OTcommut);
X1(_assoc,_OTassoc);
X1(_sideff,_OTsideff);
X1(_eop0e,_OTeop0e);
X1(_eop00,_OTeop00);
X1(_eop1e,_OTeop1e);
X2(_logical,_OTlogical);
X2(_wid,_OTwid);
X2(_call,_OTcall);
X2(_rtol,_OTrtol);
X2(_assign,_OTassign);
X2(_def,_OTdef);
X2(_ae,_OTae);
X2(_exp,_OTexp);
X3(_boolnop,_OTboolnop);
#undef X3
#undef X2
#undef X1
f = fopen("optab.c","w");
fprintf(f,"const unsigned char optab1[OPMAX] =\n\t{");
for (i = 0; i < OPMAX; i++)
{ if ((i & 7) == 0)
fprintf(f,"\n\t");
fprintf(f,"0x%x",xptab1[i]);
if (i != OPMAX - 1)
fprintf(f,",");
}
fprintf(f,"\t};\n");
fprintf(f,"const unsigned char optab2[OPMAX] =\n\t{");
for (i = 0; i < OPMAX; i++)
{ if ((i & 7) == 0)
fprintf(f,"\n\t");
fprintf(f,"0x%x",xptab2[i]);
if (i != OPMAX - 1)
fprintf(f,",");
}
fprintf(f,"\t};\n");
fprintf(f,"const unsigned char optab3[OPMAX] =\n\t{");
for (i = 0; i < OPMAX; i++)
{ if ((i & 7) == 0)
fprintf(f,"\n\t");
fprintf(f,"0x%x",xptab3[i]);
if (i != OPMAX - 1)
fprintf(f,",");
}
fprintf(f,"\t};\n");
fprintf(f,"const unsigned char opcost[OPMAX] =\n\t{");
for (i = 0; i < OPMAX; i++)
{ if ((i & 7) == 0)
fprintf(f,"\n\t");
fprintf(f,"0x%x",cost(i));
if (i != OPMAX - 1)
fprintf(f,",");
}
fprintf(f,"\t};\n");
doreltables(f);
fclose(f);
}
void StereogramWidget::paintEvent(QPaintEvent* event)
{
m_radius = 0; //means that nothing has been drawn (yet ;)
QLabel::paintEvent(event);
QPainter painter(this);
painter.setRenderHints(QPainter::Antialiasing | QPainter::TextAntialiasing, true);
//pen
QPen pen;
pen.setStyle(Qt::SolidLine);
pen.setBrush(QColor(Qt::black));
int diameter = std::min(width(),height());
int halfW = width()/2;
int halfH = height()/2;
QPoint center(halfW,halfH);
int hsvThickness = 0;
if (m_showHSVRing)
{
int newDiameter = static_cast<int>(ceil(0.9*static_cast<double>(diameter)));
hsvThickness = diameter - newDiameter;
//TODO
if (hsvThickness > 0)
{
QRect rectangle(center.x()-diameter/2+1,center.y()-diameter/2+1,diameter-2,diameter-2);
int angle_span = static_cast<int>(m_angularStep_deg * 16.0); //see QPainter::drawPie
QBrush brush;
brush.setStyle(Qt::SolidPattern);
painter.setPen(Qt::NoPen);
//dip direction steps (dip dir. in [0,360])
unsigned ddSteps = static_cast<unsigned>(ceil(360.0 / std::max(m_angularStep_deg,1.0)));
for (unsigned j=0; j<ddSteps; ++j)
{
double dipDir_deg = static_cast<double>(j) * m_angularStep_deg;
//set family color
ccColor::Rgb col;
FacetsClassifier::GenerateSubfamilyColor(col, 90.0, dipDir_deg + 0.5 * m_angularStep_deg, 0, 1);
brush.setColor(QColor( static_cast<int>(col.r),
static_cast<int>(col.g),
static_cast<int>(col.b),
255));
painter.setBrush(brush);
int angle_start = static_cast<int>((360.0 - dipDir_deg - m_angularStep_deg + 90.0) * 16.0); //see QPainter::drawPie
painter.drawPie(rectangle,angle_start,angle_span);
}
}
diameter = newDiameter;
}
//outer circle
pen.setWidth(2);
painter.setPen(pen);
painter.setBrush(Qt::white);
int radius = diameter/2 - 2;
painter.drawEllipse(center,radius,radius);
painter.setBrush(Qt::NoBrush);
//keep track of the circle position
m_radius = radius;
m_center = center;
//main axes
painter.drawLine(center-QPoint(radius,0),center+QPoint(radius,0));
painter.drawLine(center-QPoint(0,radius),center+QPoint(0,radius));
//draw circles
if (m_angularStep_deg > 0)
{
//dip steps (dip in [0,90])
unsigned dSteps = static_cast<unsigned>(ceil(90.0 / m_angularStep_deg));
//dip direction steps (dip dir. in [0,360])
unsigned ddSteps = static_cast<unsigned>(ceil(360.0 / m_angularStep_deg));
//draw inner circles
pen.setWidth(1);
pen.setColor(Qt::gray);
painter.setPen(pen);
for (unsigned i=1; i<dSteps; ++i)
{
double dip_deg = static_cast<double>(i) * m_angularStep_deg;
if (dip_deg < 90.0)
{
int R = static_cast<int>(static_cast<double>(radius) * (dip_deg/90.0));
if (R > 1)
painter.drawEllipse(center,R-1,R-1);
}
}
//draw rays (+ 'm_ticksFreq' times more ticks)
int ticksFreq = std::max(m_ticksFreq,1);
for (unsigned j=1; j<=ddSteps*ticksFreq; ++j)
{
double dipDir_deg = static_cast<double>(j) * m_angularStep_deg / static_cast<double>(ticksFreq);
if (dipDir_deg < 360.0)
{
QPoint X( static_cast<int>(sin(dipDir_deg * CC_DEG_TO_RAD) * static_cast<double>(radius)),
-static_cast<int>(cos(dipDir_deg * CC_DEG_TO_RAD) * static_cast<double>(radius)) );
if ((j % ticksFreq) == 0) //long ticks
painter.drawLine(center,center+X);
else
painter.drawLine(center+X*0.93,center+X);
}
}
}
//draw density map
if (m_densityGrid && m_densityColorScale && m_densityGrid->grid && m_densityGrid->minMaxDensity[1] != 0)
{
assert(m_densityColorScale);
assert(m_densityGrid->grid);
QBrush brush;
brush.setStyle(Qt::SolidPattern);
painter.setPen(Qt::NoPen);
QPolygon poly(4);
const double* d = m_densityGrid->grid;
for (unsigned j=0; j<m_densityGrid->ddSteps; ++j)
{
double dipDir0_rad = static_cast<double>(j) * m_densityGrid->step_deg * CC_DEG_TO_RAD;
double dipDir1_rad = static_cast<double>(j+1) * m_densityGrid->step_deg * CC_DEG_TO_RAD;
double cos_dipDir0 = cos(dipDir0_rad);
double sin_dipDir0 = sin(dipDir0_rad);
double cos_dipDir1 = cos(dipDir1_rad);
double sin_dipDir1 = sin(dipDir1_rad);
for (unsigned i=0; i<m_densityGrid->rSteps; ++i, ++d)
{
if (*d != 0)
{
double relPos = static_cast<double>(*d)/static_cast<double>(m_densityGrid->minMaxDensity[1]);
const colorType* col = m_densityColorScale->getColorByRelativePos(relPos,m_densityColorScaleSteps);
brush.setColor(QColor( static_cast<int>(col[0]),
static_cast<int>(col[1]),
static_cast<int>(col[2]),
255));
painter.setBrush(brush);
//stereographic projection
//double dip0_rad = static_cast<double>(i) * m_densityGrid->step_deg * CC_DEG_TO_RAD;
//double dip1_rad = static_cast<double>(i+1) * m_densityGrid->step_deg * CC_DEG_TO_RAD;
//double R0 = static_cast<double>(radius) * cos(dip0_rad) / (1.0 + sin(dip0_rad));
//double R1 = static_cast<double>(radius) * cos(dip1_rad) / (1.0 + sin(dip1_rad));
double R0 = static_cast<double>(radius) * static_cast<double>(i) * m_densityGrid->step_R;
double R1 = static_cast<double>(radius) * static_cast<double>(i+1) * m_densityGrid->step_R;
poly.setPoint(0,center+QPoint(static_cast<int>(sin_dipDir0 * R0),-static_cast<int>(cos_dipDir0 * R0)));
poly.setPoint(1,center+QPoint(static_cast<int>(sin_dipDir0 * R1),-static_cast<int>(cos_dipDir0 * R1)));
poly.setPoint(2,center+QPoint(static_cast<int>(sin_dipDir1 * R1),-static_cast<int>(cos_dipDir1 * R1)));
poly.setPoint(3,center+QPoint(static_cast<int>(sin_dipDir1 * R0),-static_cast<int>(cos_dipDir1 * R0)));
painter.drawPolygon(poly);
}
}
}
}
//draw main 'dip direction'
if (m_meanDipDir_deg >= 0)
{
pen.setWidth(2);
pen.setColor(Qt::red);
painter.setPen(pen);
//draw main direction
QPoint X( static_cast<int>(sin(m_meanDipDir_deg * CC_DEG_TO_RAD) * static_cast<double>(radius)),
-static_cast<int>(cos(m_meanDipDir_deg * CC_DEG_TO_RAD) * static_cast<double>(radius)) );
pen.setStyle(Qt::DashLine);
painter.setPen(pen);
painter.drawLine(center,center+X);
//draw orthogonal to main direction
QPoint Y( static_cast<int>(cos(m_meanDipDir_deg * CC_DEG_TO_RAD) * static_cast<double>(radius)),
static_cast<int>(sin(m_meanDipDir_deg * CC_DEG_TO_RAD) * static_cast<double>(radius)) );
pen.setStyle(Qt::SolidLine);
painter.setPen(pen);
painter.drawLine(center-Y,center+Y);
}
//draw filter window around last cliked point
if (m_trackMouseClick)
{
pen.setWidth(2);
pen.setColor(Qt::magenta);
painter.setPen(pen);
//QBrush brush;
//brush.setStyle(Qt::Dense6Pattern);
//brush.setColor(Qt::red);
//painter.setBrush(brush);
painter.setBrush(Qt::NoBrush);
double R0 = static_cast<double>(radius) * (std::max(0.0,m_clickDip_deg-m_clickDipSpan_deg/2)/90.0);
double R1 = static_cast<double>(radius) * (std::min(90.0,m_clickDip_deg+m_clickDipSpan_deg/2)/90.0);
//draw radial limits
{
QPoint X0( static_cast<int>(sin((m_clickDipDir_deg-m_clickDipDirSpan_deg/2) * CC_DEG_TO_RAD) * R0),
-static_cast<int>(cos((m_clickDipDir_deg-m_clickDipDirSpan_deg/2) * CC_DEG_TO_RAD) * R0) );
QPoint X1( static_cast<int>(sin((m_clickDipDir_deg-m_clickDipDirSpan_deg/2) * CC_DEG_TO_RAD) * R1),
-static_cast<int>(cos((m_clickDipDir_deg-m_clickDipDirSpan_deg/2) * CC_DEG_TO_RAD) * R1) );
painter.drawLine(center+X0,center+X1);
}
{
QPoint X0( static_cast<int>(sin((m_clickDipDir_deg+m_clickDipDirSpan_deg/2) * CC_DEG_TO_RAD) * R0),
-static_cast<int>(cos((m_clickDipDir_deg+m_clickDipDirSpan_deg/2) * CC_DEG_TO_RAD) * R0) );
QPoint X1( static_cast<int>(sin((m_clickDipDir_deg+m_clickDipDirSpan_deg/2) * CC_DEG_TO_RAD) * R1),
-static_cast<int>(cos((m_clickDipDir_deg+m_clickDipDirSpan_deg/2) * CC_DEG_TO_RAD) * R1) );
painter.drawLine(center+X0,center+X1);
}
//draw concentric limits
{
int angle_start = static_cast<int>((360.0 - m_clickDipDir_deg - m_clickDipDirSpan_deg/2 + 90.0) * 16.0); //see QPainter::drawPie
int angle_span = static_cast<int>(m_clickDipDirSpan_deg * 16.0); //see QPainter::drawPie
QRectF rect0(static_cast<double>(center.x()) - R0,
static_cast<double>(center.y()) - R0,
2*R0, 2*R0);
painter.drawArc(rect0,angle_start,angle_span);
QRectF rect1(static_cast<double>(center.x()) - R1,
static_cast<double>(center.y()) - R1,
2*R1, 2*R1);
painter.drawArc(rect1,angle_start,angle_span);
}
}
}
#define FPE 1 /* take a floating point exception */
#define NOTFPU 2 /* not an FPU instruction */
/*
* Translate current exceptions into `first' exception. The
* bits go the wrong way for ffs() (0x10 is most important, etc).
* There are only 5, so do it the obvious way.
*/
#define X1(x) x
#define X2(x) x,x
#define X4(x) x,x,x,x
#define X8(x) X4(x),X4(x)
#define X16(x) X8(x),X8(x)
static char cx_to_trapx[] = {
X1(FSR_NX),
X2(FSR_DZ),
X4(FSR_UF),
X8(FSR_OF),
X16(FSR_NV)
};
static u_char fpu_codes[] = {
X1(FPE_FLTINEX_TRAP),
X2(FPE_FLTDIV_TRAP),
X4(FPE_FLTUND_TRAP),
X8(FPE_FLTOVF_TRAP),
X16(FPE_FLTOPERR_TRAP)
};
static int fpu_types[] = {
X1(FPE_FLTRES),
int Load3DS (const char * cpcName, C3DRender & rend)
{
file3ds *file= NULL;
database3ds *db = NULL;
CLEAR_ERROR;
file = OpenFile3ds(cpcName, "r");
if (file == NULL) return __LINE__;
rend.Clear ();
rend.name = cpcName;
InitDatabase3ds(&db);
if (ftkerr3ds) return __LINE__;
CreateDatabase3ds(file, db);
if (ftkerr3ds) return __LINE__;
rend.Protect (); // закрыть объект ренд
ulong3ds nbrmat = GetMaterialCount3ds(db);
if (ftkerr3ds) return __LINE__;
rend.m_materials.resize (nbrmat+1); // 0 материал будет дефолтный!
rend.m_materials[0].name = "_SMelm_def";
rend.m_materials[0].ambient.r = 0;
rend.m_materials[0].ambient.g = 0;
rend.m_materials[0].ambient.b = 0;
rend.m_materials[0].ambient.a = 0;
rend.m_materials[0].diffuse.r = 1;
rend.m_materials[0].diffuse.g = 1;
rend.m_materials[0].diffuse.b = 1;
rend.m_materials[0].diffuse.a = 1;
rend.m_materials[0].specular.r = 0;
rend.m_materials[0].specular.g = 0;
rend.m_materials[0].specular.b = 0;
rend.m_materials[0].specular.a = 0;
rend.m_materials[0].emission.r = 0;
rend.m_materials[0].emission.g = 0;
rend.m_materials[0].emission.b = 0;
rend.m_materials[0].shininess = 0;
rend.m_materials[0].shinstrength = 0;
rend.m_materials[0].selfillumpct = 0;
rend.m_materials[0].texture.map.is_loaded = false;
rend.m_materials[0].texture.map.name = "";
rend.m_materials[0].reflect.map.is_loaded = false;
rend.m_materials[0].reflect.map.name = "";
for (ulong3ds i = 0; i < nbrmat; ++i)
{
material3ds *mat=NULL;
GetMaterialByIndex3ds(db, i, &mat);
if (ftkerr3ds) return __LINE__;
S3DMaterial &_mat = rend.m_materials[i+1];
_mat.name = mat->name;
_mat.ambient.r = mat->ambient.r;
_mat.ambient.g = mat->ambient.g;
_mat.ambient.b = mat->ambient.b;
_mat.ambient.a = 1;
_mat.diffuse.r = mat->diffuse.r;
_mat.diffuse.g = mat->diffuse.g;
_mat.diffuse.b = mat->diffuse.b;
_mat.diffuse.a = 1;
_mat.specular.r = mat->specular.r * mat->shinstrength;
_mat.specular.g = mat->specular.g * mat->shinstrength;
_mat.specular.b = mat->specular.b * mat->shinstrength;
_mat.specular.a = 1;
_mat.shininess = mat->shininess;
_mat.shinstrength = mat->shinstrength;
_mat.selfillumpct = mat->selfillumpct;
_mat.emission.r = 0.01f * _mat.selfillumpct;
_mat.emission.g = 0.01f * _mat.selfillumpct;
_mat.emission.b = 0.01f * _mat.selfillumpct;
_mat.emission.a = 1;
_mat.texture.map.is_loaded = false;
_mat.texture.map.name = mat->texture.map.name;
_mat.reflect.map.is_loaded = false;
_mat.reflect.map.name = mat->reflect.map.name;
ReleaseMaterial3ds(&mat);
}
ulong3ds nbrmesh = GetMeshCount3ds(db);
if (ftkerr3ds) return __LINE__;
rend.m_objects.resize (nbrmesh+1);
// нужен объект типа ROOT нулевой
rend.m_objects[0].name = "";
rend.m_objects[0].parent_name = "__NO__PARENT_MELMAN__";
rend.m_objects[0].parent_index = -1;
for (ulong3ds i = 0; i < nbrmesh; ++i)
{
mesh3ds *mesh=NULL;
GetMeshByIndex3ds(db, i, &mesh);
if (ftkerr3ds) return __LINE__;
S3DObject & _obj = rend.m_objects[i+1];
// скопировать имя
_obj.name = mesh->name;
_obj.parent_index = 0;
// скопирвоать вершины
_obj.vertexarray.resize (mesh->nvertices);
for (unsigned int k = 0; k < mesh->nvertices; ++k)
{
_obj.vertexarray[k].Set (mesh->vertexarray[k].x, mesh->vertexarray[k].y, mesh->vertexarray[k].z);
}
// скопировать текстурные координаты
_obj.textarray.resize (mesh->ntextverts);
for (unsigned int k = 0; k < mesh->ntextverts; ++k)
{
_obj.textarray[k].u = mesh->textarray[k].u;
_obj.textarray[k].v = mesh->textarray[k].v;
}
// задать локальную сист. координат.
_obj.localCS.H.LoadIdentity ();
C3DVectorF X1(mesh->locmatrix[0 * 3 + 0], mesh->locmatrix[0 * 3 + 1], mesh->locmatrix[0 * 3 + 2]);
C3DVectorF X2(mesh->locmatrix[1 * 3 + 0], mesh->locmatrix[1 * 3 + 1], mesh->locmatrix[1 * 3 + 2]);
C3DVectorF X3(mesh->locmatrix[2 * 3 + 0], mesh->locmatrix[2 * 3 + 1], mesh->locmatrix[2 * 3 + 2]);
C3DVectorF O (mesh->locmatrix[3 * 3 + 0], mesh->locmatrix[3 * 3 + 1], mesh->locmatrix[3 * 3 + 2]);
_obj.localCS.H_from_zero.LoadInverseTransform (X1, X2, X3, O); // загрузить матрицу трансформации из 0!
_obj.localCS.H_from_zero.Transpose (); // транспонировать для OpenGL - нужно ли???
_obj.localCS.H_to_zero = _obj.localCS.H_from_zero.Invert (); // матрица возврата в 0
_obj.localCS.pivot.Set (0,0,0); //
// распределить меши
if (mesh->nmats > 0)
{
_obj.meshes.resize (mesh->nmats);// количество подмешей.
for (unsigned int k = 0; k < mesh->nmats; ++k)
{
const objmat3ds & omat = mesh->matarray[k];
_obj.meshes[k].matname = omat.name; // скопировать имя и найти локальный индекс материала используемый подмешем.
_obj.meshes[k].MatIndex = rend.GetMaterialIndex (omat.name); // если не будет найдено имя материала то будем использовать 0 как дефолтный.
// скопировать фейсы подмеша
_obj.meshes[k].facearray.resize (omat.nfaces);
for (unsigned int j = 0; j < omat.nfaces; ++j)
{
_obj.meshes[k].facearray[j].V1 = mesh->facearray[omat.faceindex[j]].v1;
_obj.meshes[k].facearray[j].V2 = mesh->facearray[omat.faceindex[j]].v2;
_obj.meshes[k].facearray[j].V3 = mesh->facearray[omat.faceindex[j]].v3;
}
}
}
else
{ // так бывает если нет ни одного материала и нет разбивки на подмеши, нужно все засунуть в 1 меш.
_obj.meshes.resize (1);// количество подмешей.
_obj.meshes[0].matname = "_SMelm_def"; // дефолтный материал
_obj.meshes[0].MatIndex = 0; //
// скопировать фейсы подмеша
_obj.meshes[0].facearray.resize (mesh->nfaces);
for (unsigned int j = 0; j < mesh->nfaces; ++j)
{
_obj.meshes[0].facearray[j].V1 = mesh->facearray[j].v1;
_obj.meshes[0].facearray[j].V2 = mesh->facearray[j].v2;
_obj.meshes[0].facearray[j].V3 = mesh->facearray[j].v3;
}
}
RelMeshObj3ds(&mesh);
}
ulong3ds nbrobj = GetObjectNodeCount3ds(db);
if (ftkerr3ds) return __LINE__;
for (ulong3ds i = 0; i < nbrobj; ++i)
{
kfmesh3ds *omo=NULL;
GetObjectMotionByIndex3ds (db, i, &omo);
if (ftkerr3ds) return __LINE__;
std::string oname = omo->name;
if (oname == "$$$DUMMY") // блядский пустой узел
{
// его нужно внести в список объектов
S3DObject dymmy;
dymmy.name = oname + "." + omo->instance;
dymmy.localCS.pivot.Set (omo->pivot.x, omo->pivot.y, omo->pivot.z);
dymmy.localCS.H.LoadIdentity ();
dymmy.localCS.H_from_zero.LoadIdentity ();
dymmy.localCS.H_to_zero.LoadIdentity ();
dymmy.parent_name = omo->parent;
dymmy.parent_index = rend.GetObjectIndex (omo->parent);
rend.m_objects.push_back (dymmy);
}
else
{
int oi = rend.GetObjectIndex (omo->name);
if (oi != -1) // такого быть не должно иначе чушь!
{
rend.m_objects[oi].parent_name = omo->parent;
rend.m_objects[oi].parent_index = rend.GetObjectIndex (omo->parent);
rend.m_objects[oi].localCS.pivot.Set (omo->pivot.x, omo->pivot.y, omo->pivot.z);
}
}
ReleaseObjectMotion3ds (&omo);
}
ReleaseDatabase3ds(&db);
CloseFile3ds(file);
CloseAllFiles3ds ();
// рассчитать нормали для всех объектов
rend.CalcNormals ();
rend.LoadTextures ();
// структура считана и заполнена.
// теперь нужно дерево иерархии заполнить как-нибудь....
rend.m_tree.obj_index = 0;
FindAllRefsToMe (rend.m_tree, rend);
rend.UnProtect ();
return 0;
}
inline void
TrmmLLTA
( Orientation orientation,
UnitOrNonUnit diag,
T alpha,
const DistMatrix<T>& L,
DistMatrix<T>& X )
{
#ifndef RELEASE
PushCallStack("internal::TrmmLLTA");
if( L.Grid() != X.Grid() )
throw std::logic_error
("L and X must be distributed over the same grid");
if( orientation == NORMAL )
throw std::logic_error
("TrmmLLTA expects a (Conjugate)Transpose option");
if( L.Height() != L.Width() || L.Height() != X.Height() )
{
std::ostringstream msg;
msg << "Nonconformal TrmmLLTA: \n"
<< " L ~ " << L.Height() << " x " << L.Width() << "\n"
<< " X ~ " << X.Height() << " x " << X.Width() << "\n";
throw std::logic_error( msg.str().c_str() );
}
#endif
const Grid& g = L.Grid();
// Matrix views
DistMatrix<T>
LTL(g), LTR(g), L00(g), L01(g), L02(g),
LBL(g), LBR(g), L10(g), L11(g), L12(g),
L20(g), L21(g), L22(g);
DistMatrix<T>
XL(g), XR(g),
X0(g), X1(g), X2(g);
DistMatrix<T,MC,STAR> X1_MC_STAR(g);
DistMatrix<T,MR,STAR> Z1_MR_STAR(g);
DistMatrix<T,MR,MC > Z1_MR_MC(g);
X1_MC_STAR.AlignWith( L );
Z1_MR_STAR.AlignWith( L );
PartitionRight( X, XL, XR, 0 );
while( XL.Width() < X.Width() )
{
RepartitionRight
( XL, /**/ XR,
X0, /**/ X1, X2 );
Zeros( X1.Height(), X1.Width(), Z1_MR_STAR );
//--------------------------------------------------------------------//
X1_MC_STAR = X1;
LocalTrmmAccumulateLLT
( orientation, diag, alpha, L, X1_MC_STAR, Z1_MR_STAR );
Z1_MR_MC.SumScatterFrom( Z1_MR_STAR );
X1 = Z1_MR_MC;
//--------------------------------------------------------------------//
SlidePartitionRight
( XL, /**/ XR,
X0, X1, /**/ X2 );
}
#ifndef RELEASE
PopCallStack();
#endif
}
inline void
TrsmLLTSmall
( Orientation orientation, UnitOrNonUnit diag,
F alpha, const DistMatrix<F,STAR,VR>& L, DistMatrix<F,VR,STAR>& X,
bool checkIfSingular )
{
#ifndef RELEASE
PushCallStack("internal::TrsmLLTSmall");
if( L.Grid() != X.Grid() )
throw std::logic_error
("L and X must be distributed over the same grid");
if( orientation == NORMAL )
throw std::logic_error("TrsmLLT expects a (Conjugate)Transpose option");
if( L.Height() != L.Width() || L.Height() != X.Height() )
{
std::ostringstream msg;
msg << "Nonconformal TrsmLLT: \n"
<< " L ~ " << L.Height() << " x " << L.Width() << "\n"
<< " X ~ " << X.Height() << " x " << X.Width() << "\n";
throw std::logic_error( msg.str().c_str() );
}
if( L.RowAlignment() != X.ColAlignment() )
throw std::logic_error("L and X must be aligned");
#endif
const Grid& g = L.Grid();
// Matrix views
DistMatrix<F,STAR,VR>
LTL(g), LTR(g), L00(g), L01(g), L02(g),
LBL(g), LBR(g), L10(g), L11(g), L12(g),
L20(g), L21(g), L22(g);
DistMatrix<F,VR,STAR> XT(g), X0(g),
XB(g), X1(g),
X2(g);
// Temporary distributions
DistMatrix<F,STAR,STAR> L11_STAR_STAR(g);
DistMatrix<F,STAR,STAR> X1_STAR_STAR(g);
// Start the algorithm
Scale( alpha, X );
LockedPartitionUpDiagonal
( L, LTL, LTR,
LBL, LBR, 0 );
PartitionUp
( X, XT,
XB, 0 );
while( XT.Height() > 0 )
{
LockedRepartitionUpDiagonal
( LTL, /**/ LTR, L00, L01, /**/ L02,
/**/ L10, L11, /**/ L12,
/*************/ /******************/
LBL, /**/ LBR, L20, L21, /**/ L22 );
RepartitionUp
( XT, X0,
X1,
/**/ /**/
XB, X2 );
//--------------------------------------------------------------------//
L11_STAR_STAR = L11; // L11[* ,* ] <- L11[* ,VR]
X1_STAR_STAR = X1; // X1[* ,* ] <- X1[VR,* ]
// X1[* ,* ] := L11^-[T/H][* ,* ] X1[* ,* ]
LocalTrsm
( LEFT, LOWER, orientation, diag,
F(1), L11_STAR_STAR, X1_STAR_STAR, checkIfSingular );
X1 = X1_STAR_STAR;
// X0[VR,* ] -= L10[* ,VR]^(T/H) X1[* ,* ]
LocalGemm( orientation, NORMAL, F(-1), L10, X1_STAR_STAR, F(1), X0 );
//--------------------------------------------------------------------//
SlideLockedPartitionUpDiagonal
( LTL, /**/ LTR, L00, /**/ L01, L02,
/*************/ /******************/
/**/ L10, /**/ L11, L12,
LBL, /**/ LBR, L20, /**/ L21, L22 );
SlidePartitionUp
( XT, X0,
/**/ /**/
X1,
XB, X2 );
}
#ifndef RELEASE
PopCallStack();
#endif
}
inline void
TrsmLLTLarge
( Orientation orientation, UnitOrNonUnit diag,
F alpha, const DistMatrix<F>& L, DistMatrix<F>& X,
bool checkIfSingular )
{
#ifndef RELEASE
PushCallStack("internal::TrsmLLTLarge");
if( orientation == NORMAL )
throw std::logic_error("TrsmLLT expects a (Conjugate)Transpose option");
#endif
const Grid& g = L.Grid();
// Matrix views
DistMatrix<F>
LTL(g), LTR(g), L00(g), L01(g), L02(g),
LBL(g), LBR(g), L10(g), L11(g), L12(g),
L20(g), L21(g), L22(g);
DistMatrix<F> XT(g), X0(g),
XB(g), X1(g),
X2(g);
// Temporary distributions
DistMatrix<F,STAR,MC > L10_STAR_MC(g);
DistMatrix<F,STAR,STAR> L11_STAR_STAR(g);
DistMatrix<F,STAR,MR > X1_STAR_MR(g);
DistMatrix<F,STAR,VR > X1_STAR_VR(g);
// Start the algorithm
Scale( alpha, X );
LockedPartitionUpDiagonal
( L, LTL, LTR,
LBL, LBR, 0 );
PartitionUp
( X, XT,
XB, 0 );
while( XT.Height() > 0 )
{
LockedRepartitionUpDiagonal
( LTL, /**/ LTR, L00, L01, /**/ L02,
/**/ L10, L11, /**/ L12,
/*************/ /******************/
LBL, /**/ LBR, L20, L21, /**/ L22 );
RepartitionUp
( XT, X0,
X1,
/**/ /**/
XB, X2 );
L10_STAR_MC.AlignWith( X0 );
X1_STAR_MR.AlignWith( X0 );
//--------------------------------------------------------------------//
L11_STAR_STAR = L11; // L11[* ,* ] <- L11[MC,MR]
X1_STAR_VR = X1; // X1[* ,VR] <- X1[MC,MR]
// X1[* ,VR] := L11^-[T/H][* ,* ] X1[* ,VR]
LocalTrsm
( LEFT, LOWER, orientation, diag, F(1), L11_STAR_STAR, X1_STAR_VR,
checkIfSingular );
X1_STAR_MR = X1_STAR_VR; // X1[* ,MR] <- X1[* ,VR]
X1 = X1_STAR_MR; // X1[MC,MR] <- X1[* ,MR]
L10_STAR_MC = L10; // L10[* ,MC] <- L10[MC,MR]
// X0[MC,MR] -= (L10[* ,MC])^(T/H) X1[* ,MR]
// = L10^[T/H][MC,* ] X1[* ,MR]
LocalGemm
( orientation, NORMAL, F(-1), L10_STAR_MC, X1_STAR_MR, F(1), X0 );
//--------------------------------------------------------------------//
L10_STAR_MC.FreeAlignments();
X1_STAR_MR.FreeAlignments();
SlideLockedPartitionUpDiagonal
( LTL, /**/ LTR, L00, /**/ L01, L02,
/*************/ /******************/
/**/ L10, /**/ L11, L12,
LBL, /**/ LBR, L20, /**/ L21, L22 );
SlidePartitionUp
( XT, X0,
/**/ /**/
X1,
XB, X2 );
}
#ifndef RELEASE
PopCallStack();
#endif
}
inline void
TrmmLLNC
( UnitOrNonUnit diag,
T alpha, const DistMatrix<T>& L,
DistMatrix<T>& X )
{
#ifndef RELEASE
CallStackEntry entry("internal::TrmmLLNC");
if( L.Grid() != X.Grid() )
throw std::logic_error
("L and X must be distributed over the same grid");
if( L.Height() != L.Width() || L.Width() != X.Height() )
{
std::ostringstream msg;
msg << "Nonconformal TrmmLLNC: \n"
<< " L ~ " << L.Height() << " x " << L.Width() << "\n"
<< " X ~ " << X.Height() << " x " << X.Width() << "\n";
throw std::logic_error( msg.str().c_str() );
}
#endif
const Grid& g = L.Grid();
// Matrix views
DistMatrix<T>
LTL(g), LTR(g), L00(g), L01(g), L02(g),
LBL(g), LBR(g), L10(g), L11(g), L12(g),
L20(g), L21(g), L22(g);
DistMatrix<T> XT(g), X0(g),
XB(g), X1(g),
X2(g);
// Temporary distributions
DistMatrix<T,MC, STAR> L21_MC_STAR(g);
DistMatrix<T,STAR,STAR> L11_STAR_STAR(g);
DistMatrix<T,STAR,VR > X1_STAR_VR(g);
DistMatrix<T,MR, STAR> X1Trans_MR_STAR(g);
// Start the algorithm
Scale( alpha, X );
LockedPartitionUpDiagonal
( L, LTL, LTR,
LBL, LBR, 0 );
PartitionUp
( X, XT,
XB, 0 );
while( XT.Height() > 0 )
{
LockedRepartitionUpDiagonal
( LTL, /**/ LTR, L00, L01, /**/ L02,
/**/ L10, L11, /**/ L12,
/*************/ /******************/
LBL, /**/ LBR, L20, L21, /**/ L22 );
RepartitionUp
( XT, X0,
X1,
/**/ /**/
XB, X2 );
L21_MC_STAR.AlignWith( X2 );
X1Trans_MR_STAR.AlignWith( X2 );
X1_STAR_VR.AlignWith( X1 );
//--------------------------------------------------------------------//
L21_MC_STAR = L21;
X1Trans_MR_STAR.TransposeFrom( X1 );
LocalGemm
( NORMAL, TRANSPOSE, T(1), L21_MC_STAR, X1Trans_MR_STAR, T(1), X2 );
L11_STAR_STAR = L11;
X1_STAR_VR.TransposeFrom( X1Trans_MR_STAR );
LocalTrmm( LEFT, LOWER, NORMAL, diag, T(1), L11_STAR_STAR, X1_STAR_VR );
X1 = X1_STAR_VR;
//--------------------------------------------------------------------//
L21_MC_STAR.FreeAlignments();
X1Trans_MR_STAR.FreeAlignments();
X1_STAR_VR.FreeAlignments();
SlideLockedPartitionUpDiagonal
( LTL, /**/ LTR, L00, /**/ L01, L02,
/*************/ /******************/
/**/ L10, /**/ L11, L12,
LBL, /**/ LBR, L20, /**/ L21, L22 );
SlidePartitionUp
( XT, X0,
/**/ /**/
X1,
XB, X2 );
}
}
inline void
TrmmLLTCOld
( Orientation orientation,
UnitOrNonUnit diag,
T alpha,
const DistMatrix<T>& L,
DistMatrix<T>& X )
{
#ifndef RELEASE
PushCallStack("internal::TrmmLLTCOld");
if( L.Grid() != X.Grid() )
throw std::logic_error
("L and X must be distributed over the same grid");
if( orientation == NORMAL )
throw std::logic_error("TrmmLLT expects a (Conjugate)Transpose option");
if( L.Height() != L.Width() || L.Height() != X.Height() )
{
std::ostringstream msg;
msg << "Nonconformal TrmmLLTC: \n"
<< " L ~ " << L.Height() << " x " << L.Width() << "\n"
<< " X ~ " << X.Height() << " x " << X.Width() << "\n";
throw std::logic_error( msg.str().c_str() );
}
#endif
const Grid& g = L.Grid();
// Matrix views
DistMatrix<T>
LTL(g), LTR(g), L00(g), L01(g), L02(g),
LBL(g), LBR(g), L10(g), L11(g), L12(g),
L20(g), L21(g), L22(g);
DistMatrix<T> XT(g), X0(g),
XB(g), X1(g),
X2(g);
// Temporary distributions
DistMatrix<T,STAR,STAR> L11_STAR_STAR(g);
DistMatrix<T,MC, STAR> L21_MC_STAR(g);
DistMatrix<T,STAR,VR > X1_STAR_VR(g);
DistMatrix<T,MR, STAR> D1AdjOrTrans_MR_STAR(g);
DistMatrix<T,MR, MC > D1AdjOrTrans_MR_MC(g);
DistMatrix<T,MC, MR > D1(g);
// Start the algorithm
Scale( alpha, X );
LockedPartitionDownDiagonal
( L, LTL, LTR,
LBL, LBR, 0 );
PartitionDown
( X, XT,
XB, 0 );
while( XB.Height() > 0 )
{
LockedRepartitionDownDiagonal
( LTL, /**/ LTR, L00, /**/ L01, L02,
/*************/ /******************/
/**/ L10, /**/ L11, L12,
LBL, /**/ LBR, L20, /**/ L21, L22 );
RepartitionDown
( XT, X0,
/**/ /**/
X1,
XB, X2 );
L21_MC_STAR.AlignWith( X2 );
D1AdjOrTrans_MR_STAR.AlignWith( X1 );
D1AdjOrTrans_MR_MC.AlignWith( X1 );
D1.AlignWith( X1 );
Zeros( X1.Width(), X1.Height(), D1AdjOrTrans_MR_STAR );
Zeros( X1.Height(), X1.Width(), D1 );
//--------------------------------------------------------------------//
X1_STAR_VR = X1;
L11_STAR_STAR = L11;
LocalTrmm
( LEFT, LOWER, orientation, diag, T(1), L11_STAR_STAR, X1_STAR_VR );
X1 = X1_STAR_VR;
L21_MC_STAR = L21;
LocalGemm
( orientation, NORMAL,
T(1), X2, L21_MC_STAR, T(0), D1AdjOrTrans_MR_STAR );
D1AdjOrTrans_MR_MC.SumScatterFrom( D1AdjOrTrans_MR_STAR );
if( orientation == TRANSPOSE )
Transpose( D1AdjOrTrans_MR_MC.LocalMatrix(), D1.LocalMatrix() );
else
Adjoint( D1AdjOrTrans_MR_MC.LocalMatrix(), D1.LocalMatrix() );
Axpy( T(1), D1, X1 );
//--------------------------------------------------------------------//
D1.FreeAlignments();
D1AdjOrTrans_MR_MC.FreeAlignments();
D1AdjOrTrans_MR_STAR.FreeAlignments();
L21_MC_STAR.FreeAlignments();
SlideLockedPartitionDownDiagonal
( LTL, /**/ LTR, L00, L01, /**/ L02,
/**/ L10, L11, /**/ L12,
/*************/ /******************/
LBL, /**/ LBR, L20, L21, /**/ L22 );
SlidePartitionDown
( XT, X0,
X1,
/**/ /**/
XB, X2 );
}
#ifndef RELEASE
PopCallStack();
#endif
}
void
vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
{
if (DEBUG_LEVEL1)
std::cout << "begin CPose::PoseLagrange(...) " << std::endl ;
try{
double s;
int i;
int k=0;
int nl=npt*2;
vpMatrix a(nl,3) ;
vpMatrix b(nl,9);
b =0 ;
vpPoint P ;
listP.front() ;
i=0 ;
while (!listP.outside())
{
P= listP.value() ;
a[k][0] = -P.get_oX();
a[k][1] = 0.0;
a[k][2] = P.get_oX()*P.get_x();
a[k+1][0] = 0.0;
a[k+1][1] = -P.get_oX();
a[k+1][2] = P.get_oX()*P.get_y();
b[k][0] = -P.get_oY();
b[k][1] = 0.0;
b[k][2] = P.get_oY()*P.get_x();
b[k][3] = -P.get_oZ();
b[k][4] = 0.0;
b[k][5] = P.get_oZ()*P.get_x();
b[k][6] = -1.0;
b[k][7] = 0.0;
b[k][8] = P.get_x();
b[k+1][0] = 0.0;
b[k+1][1] = -P.get_oY();
b[k+1][2] = P.get_oY()*P.get_y();
b[k+1][3] = 0.0;
b[k+1][4] = -P.get_oZ();
b[k+1][5] = P.get_oZ()*P.get_y();
b[k+1][6] = 0.0;
b[k+1][7] = -1.0;
b[k+1][8] = P.get_y();
k += 2;
listP.next() ;
}
vpColVector X1(3) ;
vpColVector X2(9) ;
if (DEBUG_LEVEL2)
{
std::cout <<"a " << a << std::endl ;
std::cout <<"b " << b << std::endl ;
}
lagrange(a,b,X1,X2);
// if (err != OK)
{
// std::cout << "in (CLagrange.cc)Lagrange returns " ;
// PrintError(err) ;
// return err ;
}
if (DEBUG_LEVEL2)
{
std::cout << "ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
std::cout << "norme X1 " << X1.sumSquare() << std::endl ;;
}
if (X2[8] < 0.0)
{ /* car Zo > 0 */
X1 *= -1 ;
X2 *= -1 ;
}
s = 0.0;
for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
for (i=0;i<3;i++) {X2[i] -= (s*X1[i]);} /* X1^T X2 = 0 */
s = 0.0;
for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
if (s<1e-10)
{
vpERROR_TRACE(" division par zero " ) ;
throw(vpException(vpException::divideByZeroError,
"division by zero ")) ;
}
s = 1.0/sqrt(s);
for (i=0;i<3;i++) {X2[i] *= s;} /* X2^T X2 = 1 */
X2[3] = (X1[1]*X2[2])-(X1[2]*X2[1]);
X2[4] = (X1[2]*X2[0])-(X1[0]*X2[2]);
X2[5] = (X1[0]*X2[1])-(X1[1]*X2[0]);
calculTranslation (a, b, nl, 3, 6, X1, X2) ;
for (i=0 ; i<3 ; i++)
{
cMo[i][0] = X1[i];
cMo[i][1] = X2[i];
cMo[i][2] = X2[i+3];
cMo[i][3] = X2[i+6];
}
}
catch(...)
{
vpERROR_TRACE(" ") ;
throw ;
}
if (DEBUG_LEVEL1)
std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl ;
}
