cElHomographie cElHomographie::operator * (const cElHomographie & H2) const
{
ElMatrix<REAL> M1(3,3);
ElMatrix<REAL> M2(3,3);
ElMatrix<REAL> Res(3,3);
this->ToMatrix(M1);
H2.ToMatrix(M2);
Res.mul(M1,M2);
return FromMatrix(Res);
}
void DenseSubMatrix<T>::right_multiply (const DenseMatrixBase<T> & M3)
{
// (*this) <- M2 * M3
// Where:
// (*this) = (m x n),
// M2 = (m x p),
// M3 = (p x n)
// M2 is simply a copy of *this
DenseSubMatrix<T> M2(*this);
// Call the multiply function in the base class
this->multiply(*this, M2, M3);
}
static double
strikezHIII (MagComp component, const fault_params *fault, const magnetic_params *mag, double xi, double et, double qq, double y, double z)
{
double val;
double h = mag->dcurier;
double qd = y * sd + (fault->fdepth - h) * cd;
double K3_val = K3 (component, 1.0, xi, et, qq);
double log_re_val = log_re (component, 1.0, xi, et, qq);
double M2_val = M2 (component, 1.0, xi, et, qq);
double M3_val = M3 (component, 1.0, xi, et, qq);
val = alpha5 * K3_val + fault->alpha * log_re_val * cd
+ alpha3 * (qd * M3_val - (z - h) * M2_val * sd);
return val;
}
flag CPipeTerm::GetModelAction(CMdlActionArray & Acts)
{
//CMdlAction {MAT_NULL, MAT_State, MAT_Value};
CMdlAction M0(0, MAT_State, !m_VPB.On(), "Open", 1);
CMdlAction M1(1, MAT_State, m_VPB.On(), "Close", 0);
CMdlAction M2(2, MAT_Value, m_VPB.On(), "Manual Posn (%)", true,
m_VPB.ManualPosition(this)*100, 0.0, 100.0,
m_VPB.ActualPosition(this, &m_FRB)*100);
Acts.SetSize(0);
Acts.SetAtGrow(0, M0);
Acts.SetAtGrow(1, M1);
Acts.SetAtGrow(2, M2);
Acts.SetAtGrow(3, CMdlAction(3, MAT_Switch));
return True;
};
static double
strikeyHIII (MagComp component, const fault_params *fault, const magnetic_params *mag, double xi, double et, double qq, double y, double z)
{
double val;
double h = mag->dcurier;
double qd = y * sd + (fault->fdepth - h) * cd;
double K2_val = K2 (component, 1.0, xi, et, qq);
double log_re_val = log_re (component, 1.0, xi, et, qq);
double J2_val = J2 (component, 1.0, xi, et, qq);
double L2_val = L2 (component, 1.0, xi, et, qq);
double M2_val = M2 (component, 1.0, xi, et, qq);
val = - alpha4 * K2_val - alpha4 * log_re_val * sd - alpha3 * J2_val
+ alpha3 * (qd * M2_val + (z - h) * L2_val * sd * cd);
return val;
}
VRDisplayNode* VRLookAtNode::create(VRMainInterface *vrMain, VRDataIndex *config, const std::string &nameSpace) {
VRMatrix4 lookAtMatrix;
if (config->exists("LookAtMatrix", nameSpace)){
lookAtMatrix = config->getValue("LookAtMatrix", nameSpace);
}
else if (config->exists("LookAtUp", nameSpace) && config->exists("LookAtEye", nameSpace) && config->exists("LookAtCenter", nameSpace))
{
VRVector3 up = config->getValue("LookAtUp", nameSpace);
VRVector3 eye = config->getValue("LookAtEye", nameSpace);
VRVector3 center = config->getValue("LookAtCenter", nameSpace);
VRVector3 z = center - eye;
z = z.normalize();
VRVector3 x = up.cross(z);
x = x.normalize();
VRVector3 y = z.cross(x);
VRMatrix4 M1( x[0], y[0], z[0], 0,
x[1], y[1], z[1], 0,
x[2], y[2], z[2], 0,
0, 0, 0, 1);
VRMatrix4 M2( 1, 0, 0, -eye[0],
0, 1, 0, -eye[1],
0, 0, 1, -eye[2],
0, 0, 0, 1);
lookAtMatrix = M1 * M2;
}
else
{
std::cerr << "Warning : no LookAtMatrix defined for " << nameSpace << std::endl;
std::cerr << "Either Define LookAtMatrix or LookAtUp,LookAEye and LookAtCenter" << std::endl;
std::cerr << "Using IdentityMatrix as default 1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1 " << std::endl;
}
VRLookAtNode *node = new VRLookAtNode(nameSpace, lookAtMatrix);
return node;
}
void DenseMatrix<T>::right_multiply (const DenseMatrixBase<T>& M3)
{
if (this->use_blas_lapack)
this->_multiply_blas(M3, RIGHT_MULTIPLY);
else
{
// (*this) <- M3 * (*this)
// Where:
// (*this) = (m x n),
// M2 = (m x p),
// M3 = (p x n)
// M2 is a copy of *this before it gets resize()d
DenseMatrix<T> M2(*this);
// Resize *this so that the result can fit
this->resize (M2.m(), M3.n());
this->multiply(*this, M2, M3);
}
}
TEST(ResMonoidDense, create)
{
ResMonoidDense M1(4,
std::vector<int>{1, 1, 1, 1},
std::vector<int>{},
MonomialOrderingType::GRevLex);
ResMonoidDense M2(4,
std::vector<int>{1, 2, 3, 4},
std::vector<int>{1, 1, 1, 1, 1, 1, 0, 0},
MonomialOrderingType::Weights);
ResMonoidDense M3(4,
std::vector<int>{1, 1, 1, 1},
std::vector<int>{},
MonomialOrderingType::Lex);
EXPECT_EQ(4, M1.n_vars());
std::vector<res_monomial_word> monomspace(100);
EXPECT_EQ(100, monomspace.size());
}
static double
strikezH0 (MagComp component, const fault_params *fault, const magnetic_params *mag, double xi, double et, double qq, double y, double z)
{
double val;
double h = mag->dcurier;
double qd = y * sd + (fault->fdepth - h) * cd;
double K3_val = K3 (component, 1.0, xi, et, qq);
double log_re_val = log_re (component, 1.0, xi, et, qq);
double M2_val = M2 (component, 1.0, xi, et, qq);
double M3_val = M3 (component, 1.0, xi, et, qq);
double M3y_val = M3y (component, 1.0, xi, et, qq);
double M3z_val = M3z (component, 1.0, xi, et, qq);
val = - (2.0 + alpha5) * K3_val
- fault->alpha * log_re_val * cd
- alpha3 * (qd * M3_val - (z - h) * M2_val * sd)
+ 2.0 * fault->alpha * h * (M3_val * cd + M2_val * sd)
- 4.0 * alpha1 * h * M2_val * sd
- 2.0 * alpha2 * h * M2_val * sd
- 2.0 * alpha2 * h * ((qd + h * cd) * M3z_val - (z - 2.0 * h) * M3y_val * sd);
return val;
}
int main()
{
try {
vpMatrix M ;
vpMatrix M1(2,3) ;
vpMatrix M2(3,3) ;
vpMatrix M3(2,2) ;
vpTRACE("test matrix size in multiply") ;
try
{
M = M1*M3 ;
}
catch (vpMatrixException me)
{
std::cout << me << std::endl ;
}
vpTRACE("test matrix size in addition") ;
try
{
M = M1+M3 ;
}
catch (vpMatrixException me)
{
std::cout << me << std::endl ;
}
}
catch(vpException e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
int main(void){
int //a[N], //a[= 0] is illegal
i, j, k, m;
#ifdef N //true
i = j;
#else
j = i;
#endif
i = 10 * INC(j);
//i = SUB(j, k); //doesn't work because space in definition
i = SQR(SQR(j));
i = CUBE(j);
//i = M1(j, k); //i = jk; jk is undeclared
puts(M2(i, j)); //"i""j" automatically concatenated
#undef SQR
//i = SQR(j); //undefined macro
#define SQR
//i = SQR(j); //no parameterized macro
return 0;
}
//declared the function for rho3 from rho2
double rho3(double r, double m, double p)
{
double Fn = rho(r+h/2,m+(h/2)*M2(r,m,p),p+(h/2)*rho2(r,m,p));
return Fn;
}
void main() //main code
{
printf("\nRUNGE-KUTTA METHOD\n\nR M Rho\n"); //printing titles for values displayed
double c = 10.0; //the parameter rho(c)
int n; //the integer steps, n
//declare arrays
float x[N];
float y[N];
float z[N];
//r,m,p as the radius, mass and density
double r,m,p;
//declare initial conditons for arrays
x[0] = h; //first array is for r=h
y[0] = (h*h*h/3)*c; //initial conditon for scaled mass (m)
z[0] = c*(1-((h*h*c)/(6*gamma(c)))); //initial conditon for rho
//for loop for n=0,1,...,200
for(n=0;n<N;n++)
{
//declared how x(n+1) relates to x(n), y(n+1) relates to y(n), z(n+1) relates to z(n)
x[n+1] = x[n]+h;
y[n+1] = y[n]+(h/6)*(M(x[n],y[n],z[n])+2*M2(x[n],y[n],z[n])+2*M3(x[n],y[n],z[n])+M4(x[n],y[n],z[n]));
z[n+1] = z[n]+(h/6)*(rho(x[n],y[n],z[n])+2*rho2(x[n],y[n],z[n])+2*rho3(x[n],y[n],z[n])+rho4(x[n],y[n],z[n]));
if(isnan(z[n+1]))
{
break;
}
//r,m,p will be declared in pg-plot
r = x[n+1];
m = y[n+1];
p = z[n+1];
printf("%.2e %.2e %.2e\n",x[n+1],y[n+1],z[n+1]); //printed values for x and y respectively
}
//Use pg-plot to plot mass and density
// cpgbeg starts a plotting page, in this case with 2x1 panels
cpgbeg(0,"?",2,1);
// sets colour: 1-black, 2-red, 3-green, 4-blue
cpgsci(1);
// sets line style: 1-solid, 2-dashed, 3-dot-dashed, 4-dotted
cpgsls(1);
// sets charachter height, larger number = bigger
cpgsch(1.);
// cpgpage() moves to the next panel
cpgpage();
// sets the axes limits in the panel
cpgswin(0,r,0,m);
// draw the axes
cpgbox("BCNST", 0.0, 0, "BCNST", 0.0, 0);
// label the bottom axis
cpgmtxt("B",2.,.5,.5,"radius");
// label the left axis
cpgmtxt("L",2.5,.5,.5,"saclaed mass");
// connect N points in ax and ay with a line
cpgline(n,x,y);
// cpgpage() moves to the next panel
cpgpage();
// sets the axes limits in the panel
cpgswin(0,r,0,c);
// draw the axes
cpgbox("BCNST", 0.0, 0, "BCNST", 0.0, 0);
// label the bottom axis
cpgmtxt("B",2.,.5,.5,"radius");
// label the left axis
cpgmtxt("L",2.5,.5,.5,"density");
// connect N points in ax and ay with a line
cpgline(n,x,z);
// close all pgplot applications
cpgend();
// end program
return;
}
// RUN: %clang_cc1 -E %s | grep 'ei_1 = (17 +1);' // RUN: %clang_cc1 -E %s | grep 'ei_2 = (M1)(17);' #define M1(a) (a+1) #define M2(b) b int ei_1 = M2(M1)(17); /* becomes int ei_1 = (17+1); */ int ei_2 = (M2(M1))(17); /* becomes int ei_2 = (M1)(17); */
void main() //main code
{ //printing titles for values displayed
printf("\nRUNGE-KUTTA METHOD\nstep size R M Last Rho iterations\n");
double i; //power order for rho(c)
double c = 10; //the parameter rho(c)
int n; //the integer steps, n
//declare arrays
float x[N];
float y[N];
float z[N];
float x2[N];
float y2[N];
float z2[N];
//r,m,p as the radius, mass and density
double r,m,p,r2,m2,p2;
for(i=-5;i<2;i++)
{
h = pow(10,i); //how h is varied by i
c = 10; //the parameter rho(c)
//declare initial conditons for arrays
x[0] = h; //first array is for r=h
y[0] = (h*h*h/3)*c; //initial conditon for scaled mass (m)
z[0] = c*(1-((h*h*c)/(6*gamma(c)))); //initial conditon for rho
//for loop for n=0,1,...,when z[n]>1
for(n=0;z[n]>1;n++)
{
//declared how x(n+1) relates to x(n), y(n+1) relates to y(n), z(n+1) relates to z(n)
x[n+1] = x[n]+h;
y[n+1] = y[n]+(h/6)*(M(x[n],y[n],z[n])+2*M2(x[n],y[n],z[n])+2*M3(x[n],y[n],z[n])+M4(x[n],y[n],z[n]));
z[n+1] = z[n]+(h/6)*(rho(x[n],y[n],z[n])+2*rho2(x[n],y[n],z[n])+2*rho3(x[n],y[n],z[n])+rho4(x[n],y[n],z[n]));
//r,m,p will be declared for printf statement
r = x[n];
m = y[n];
p = z[n];
}
//printed values for x and y respectively
printf("%.2e %.2e %.2e %.2e %i\n",h,r,m,p,n);
}
//printing titles for values displayed
printf("\nEULER METHOD\nstep size R M Last Rho iterations\n");
for(i=-5;i<2;i++)
{
h = pow(10,i); //how h is varied by i
c = 10; //the parameter rho(c)
//declare initial conditons for arrays
x2[0] = h; //first array is for r=h
y2[0] = (h*h*h/3)*c; //initial conditon for scaled mass (m)
z2[0] = fabs(c*(1-((h*h*c)/(6*gamma(c))))); //initial conditon for rho
//for loop for n=0,1,..., when z2[n]>1
for(n=0;z2[n]>1;n++)
{
//declared how x2(n+1) relates to x2(n), y2(n+1) relates to y2(n), z2(n+1) relates to z2(n)
x2[n+1] = x2[n]+h;
y2[n+1] = y2[n]+h*(M(x2[n],y2[n],z2[n]));
z2[n+1] = z2[n]+h*(rho(x2[n],y2[n],z2[n]));
//r2,m2,p2 will be declared in pg-plot
r2 = x2[n];
m2 = y2[n];
p2 = z2[n];
}
//printed values for x and y respectively
printf("%.2e %.2e %.2e %.2e %i\n",h,r2,m2,p2,n);
}
}
static cElComposHomographie ComputeHomFromHomologues
(
const ElPackHomologue & aPack,
bool aL2,
cElComposHomographie& aHX,
cElComposHomographie& aHY,
cElComposHomographie& aHZ
)
{
ElPackHomologue::const_iterator anIt=aPack.begin();
// 0 Point : identite
if (aPack.size() ==0)
{
aHX = cElComposHomographie(1,0,0);
aHY = cElComposHomographie(0,1,0);
aHZ = cElComposHomographie(0,0,1);
return aHX;
}
// 1 Point : translation
Pt2dr P1 = anIt->P1();
Pt2dr P2 = anIt->P2();
anIt++;
if (aPack.size() ==1)
{
Pt2dr aTr = P2 - P1;
aHX = cElComposHomographie(1,0,aTr.x);
aHY = cElComposHomographie(0,1,aTr.y);
aHZ = cElComposHomographie(0,0,1);
return aHX;
}
// 2 Point : Similitude
Pt2dr Q1 = anIt->P1();
Pt2dr Q2 = anIt->P2();
anIt++;
if (aPack.size() ==2)
{
Pt2dr W = (Q2-P2) / (Q1-P1);
Pt2dr aTr = P2 - W* P1;
aHX = cElComposHomographie(W.x,-W.y,aTr.x);
aHY = cElComposHomographie(W.y,W.x,aTr.y);
aHZ = cElComposHomographie(0,0,1);
return aHX;
}
// 3 Point : Affinite
Pt2dr R1 = anIt->P1();
Pt2dr R2 = anIt->P2();
anIt++;
if (aPack.size() ==3)
{
ElMatrix<REAL> M1(2,2); SetCol(M1,0,Q1-P1); SetCol(M1,1,R1-P1);
ElMatrix<REAL> M2(2,2); SetCol(M2,0,Q2-P2); SetCol(M2,1,R2-P2);
ElMatrix<REAL> M = M2 * gaussj(M1);
Pt2dr aTr = P2 - M* P1;
aHX = cElComposHomographie(M(0,0),M(1,0),aTr.x);
aHY = cElComposHomographie(M(0,1),M(1,1),aTr.y);
aHZ = cElComposHomographie(0,0,1);
return aHX;
}
cGenSysSurResol * aSys =
aL2 ?
( cGenSysSurResol *) new L2SysSurResol(8) :
( cGenSysSurResol *) new SystLinSurResolu(8,aPack.size());
aSys->SetPhaseEquation(0);
for ( anIt=aPack.begin() ; anIt !=aPack.end(); anIt++)
{
AddCoeffSysHom(aSys,anIt->P1(),anIt->P2(),anIt->Pds(),true );
AddCoeffSysHom(aSys,anIt->P1(),anIt->P2(),anIt->Pds(),false);
}
Im1D_REAL8 aSol = aSys->GSSR_Solve((bool *)0);
REAL * aS = aSol.data();
aHX = cElComposHomographie(aS[0],aS[1],aS[2]);
aHY = cElComposHomographie(aS[3],aS[4],aS[5]);
aHZ = cElComposHomographie(aS[6],aS[7], 1 );
delete aSys;
return aHX;
}
int f9() { return M2(1); }
void main() //main code
{
double Rs = Y*((7.72*pow(10,8))/(6.95*pow(10,10)));
double Ms = Y*Y*((5.67*pow(10,33))/(1.98*pow(10,33)));
double c = 82; //the parameter rho(c)
printf("\nSIRIUS B\n");
printf("\nRunge-Kutta Method\nMs (solar masses) Rs (solar radii) Rho(c) (solar densities) Y(e)\n"); //printing titles for values displayed
int n; //the integer steps, n
//declare arrays
float x[N];
float y[N];
float z[N];
float x2[N];
float y2[N];
float z2[N];
//r,m,p as the radius, mass and density
double r,m,p,r2,m2,p2;
//declare initial conditons for arrays
x[0] = h; //first array is for r=h
y[0] = (h*h*h/3)*c; //initial conditon for scaled mass (m)
z[0] = c*(1-((h*h*c)/(6*gamma(c)))); //initial conditon for rho
//for loop for n=0,1,...,N
for(n=0;n<N;n++)
{
//declared how x(n+1) relates to x(n), y(n+1) relates to y(n), z(n+1) relates to z(n)
x[n+1] = x[n]+h;
y[n+1] = y[n]+(h/6)*(M(x[n],y[n],z[n])+2*M2(x[n],y[n],z[n])+2*M3(x[n],y[n],z[n])+M4(x[n],y[n],z[n]));
z[n+1] = z[n]+(h/6)*(rho(x[n],y[n],z[n])+2*rho2(x[n],y[n],z[n])+2*rho3(x[n],y[n],z[n])+rho4(x[n],y[n],z[n]));
if(isnan(z[n+1]))
{
break;
}
//r,m,p will be declared in pg-plot
r = x[n+1];
m = y[n+1];
p = z[n+1];
}
printf("%.3e %.3e %.2f %.3f\n",y[n]*Ms,x[n]*Rs,c,Y); //printed values for x and y respectively
printf("\nEuler Method\nMs (solar masses) Rs (solar radii) Rho(c) (solar densities) Y(e)\n"); //printing titles for values displayed
//declare initial conditons for arrays
x2[0] = h; //first array is for r=h
y2[0] = (h*h*h/3)*c; //initial conditon for scaled mass (m)
z2[0] = c*(1-((h*h*c)/(6*gamma(c)))); //initial conditon for rho
//for loop for n=0,1,...,200
for(n=0;n<N;n++)
{
//declared how x2(n+1) relates to x2(n), y2(n+1) relates to y2(n), z2(n+1) relates to z2(n)
x2[n+1] = x2[n]+h;
y2[n+1] = y2[n]+h*(M(x2[n],y2[n],z2[n]));
z2[n+1] = z2[n]+h*(rho(x2[n],y2[n],z2[n]));
if(z2[n+1]<0)
{
break;
}
//r2,m2,p2 will be declared in pg-plot
r2 = x2[n+1];
m2 = y2[n+1];
p2 = z2[n+1];
}
//printed values for x and y respectively
printf("%.3e %.3e %.2f %.3f\n",y2[n]*Ms,x2[n]*Rs,c,Y);
}
SEXP wink_both_ser( SEXP data1, SEXP data2, SEXP windows, SEXP Rdelta, SEXP RB) throw()
{
parameters param;
if( !param.load(data1,data2,windows,Rdelta,RB) )
return R_NilValue;
//==========================================================================
// create the return matrix
//==========================================================================
SEXP Rval;
PROTECT(Rval = allocMatrix(REALSXP,2,param.num_windows) );
double *ans = REAL(Rval);
try
{
//======================================================================
// transform R data intro C++ objects
//======================================================================
wink::c_matrix M1(param.nrow1,param.ncol1);
wink::c_matrix M2(param.nrow2,param.ncol2);
M1.loadR( REAL(data1) );
M2.loadR( REAL(data2) );
//======================================================================
// make C++ neuro_pair, init random generator
//======================================================================
wink::neuro_pair NP(M1,M2,param.B);
NP.g.seed( wink::neuro_pair::shared_seed + uint32_t(time(NULL)) );
//======================================================================
// outer loop on windows
//======================================================================
for( size_t i=0; i < param.num_windows; ++i )
{
const size_t i2 = i*2;
const size_t i3 = i2+1;
//------------------------------------------------------------------
// get the boundaries
//------------------------------------------------------------------
const double a = param.ptr_windows[i2];
const double b = param.ptr_windows[i3];
//------------------------------------------------------------------
// call the integrated function
//------------------------------------------------------------------
double &pvalue_geq = ans[ i2 ];
double &pvalue_leq = ans[ i3 ];
//Rprintf("[%10.6f,%10.6f] : true_coinc=%6u : pvalue= %.8f\n",a,b,unsigned(NP.true_coinc),pvalue);
NP.both_pvalues(pvalue_geq,pvalue_leq,a,b,param.delta);
}
UNPROTECT(1);
return Rval;
}
catch(...)
{
Rprintf("Exception in code");
}
return R_NilValue;
}
// Output : center and dRadius
static void GetCenterRadiusFrom3Points2D(pcl::PointXYZ& center, double &dRadius,
const pcl::PointXYZ& A, const pcl::PointXYZ& B, const pcl::PointXYZ& C)
{
Eigen::MatrixXf M1(3,3);
M1(0,0) = A.x*A.x+A.y*A.y;
M1(0,1) = A.y;
M1(0,2) = 1;
M1(1,0) = B.x*B.x+B.y*B.y;
M1(1,1) = B.y;
M1(1,2) = 1;
M1(2,0) = C.x*C.x+C.y*C.y;
M1(2,1) = C.y;
M1(2,2) = 1;
Eigen::MatrixXf M2(3,3);
M2(0,0) = A.x;
M2(0,1) = A.y;
M2(0,2) = 1;
M2(1,0) = B.x;
M2(1,1) = B.y;
M2(1,2) = 1;
M2(2,0) = C.x;
M2(2,1) = C.y;
M2(2,2) = 1;
center.x = M1.determinant()/(2*M2.determinant());
M1(0,1) = A.x*A.x+A.y*A.y;
M1(0,0) = A.x;
M1(0,2) = 1;
M1(1,1) = B.x*B.x+B.y*B.y;
M1(1,0) = B.x;
M1(1,2) = 1;
M1(2,1) = C.x*C.x+C.y*C.y;
M1(2,0) = C.x;
M1(2,2) = 1;
center.y = M1.determinant()/(2*M2.determinant());
/*
// Takes the bisections of AB and AC and computes their intersection
Eigen::Vector2f AB;
Eigen::Vector2f AC;
AB << (B.x - A.x), (B.y - A.y);
AC << (C.x - A.x), (C.y - A.y);
Eigen::Vector2f I1, I2;
I1 << (B.x + A.x)/2, (B.y + A.y)/2;
I2 << (C.x + A.x)/2, (C.y + A.y)/2;
// a1*(y-y_i1)-b1*(x-x_i1)=0 where i1 is the middle of AB
// a2*(y-y_i2)-b2*(x-x_i2)=0 where i2 is the middle of AC
double a1 = -AB.y();
double b1 = AB.x();
double a2 = -AC.y();
double b2 = AC.x();
if(a1 == 0)
{
center.x = I1.x();
center.y = b2*(center.x-I2.x())/a2+I2.y(); // We assume AB and AC are not //. Hence a1 and a2 can't be both null.
}
else
{
center.x = (-b2*I2.x()+a2*I1.x()/a1+a2*(I2.y()-I1.y()))/(a2*b1/a1-b2);
center.y = b1*(center.x-I1.x())/a1+I1.y();
}
*/
center.z = (A.z+B.z+C.z)/3;
dRadius = (sqrt( (A.x-center.x)*(A.x-center.x) + (A.y-center.y)*(A.y-center.y) + (A.z-center.z)*(A.z-center.z) )
+ sqrt( (B.x-center.x)*(B.x-center.x) + (B.y-center.y)*(B.y-center.y) + (B.z-center.z)*(B.z-center.z) )
+sqrt( (C.x-center.x)*(C.x-center.x) + (C.y-center.y)*(C.y-center.y) + (C.z-center.z)*(C.z-center.z) ))/3;
}
pair< bool , Word > FreeMetabelianGroupAlgorithms::conjugate( int N , Word w1 , Word w2 )
{
// A. Compute the tails for w1 and w2
vector< int > T1 = getTail( N , w1 );
vector< int > T2 = getTail( N , w2 );
// B. If tails are different then the answer is no
if( T1!=T2 )
return pair< bool , Word >( false , Word() );
//----------------------------------------------------------
// C. If the tails are trivial
if( T1==vector< int >( N, 0 ) ) {
// 1. Compute the edge-maps and check if they are of the same size
map< vector< int > , int > EM1 = dropTrivialEdgesInMap( getEdgeMap( N , w1 ) );
map< vector< int > , int > EM2 = dropTrivialEdgesInMap( getEdgeMap( N , w2 ) );
if( EM1.size( )!=EM2.size( ) )
return pair< bool , Word >( false , Word() );
// 2. Check if the elements are trivial
if( EM1.size( )==0 )
return pair< bool , Word >( true , Word() );
// 3. Determine the required shift
pair< vector< int > , int > C1 = *EM1.begin( );
pair< vector< int > , int > C2 = *EM2.begin( );
vector< int > S( N , 0 );
for( int i=0 ; i<N ; ++i )
S[i] = C2.first[i]-C1.first[i];
EM1 = shiftEdgeMap( N , S , EM1 );
if( EM1==EM2)
return pair< bool , Word >( true , -getTailWord( N , S ) );
else
return pair< bool , Word >( false , Word() );
}
//----------------------------------------------------------
// D. Move the space so that x1-component is positive and others are non-negative
vector< Word > Images(N);
for( int j=0 ; j<N ; ++j )
Images[j] = Word( j+1 );
if( T1[0]==0 ) {
// 1. Find the first non-trivial component xi and swap(x1,xi)
for( int i=1 ; i<N ; ++i ) {
if( T1[i]!=0 ) {
Images[0] = Word( i+1 );
Images[i] = Word( 1 );
swap( T1[0] , T1[i] );
break;
}
}
}
Map M1( N , N , Images );
w1 = M1.imageOf( w1 );
w2 = M1.imageOf( w2 );
//----------------------------------------------------------
// E. Make negative components positive
vector< Word > Images2( N );
for( int i=0 ; i<N ; ++i ) {
if( T1[i]>=0 )
Images2[i] = Word(+i+1);
else
Images2[i] = Word(-i-1);
T1[i] = abs(T1[i]);
}
Map M2( N , N , Images2 );
w1 = M2.imageOf( w1 );
w2 = M2.imageOf( w2 );
//----------------------------------------------------------
// F. Compute the strip-forms for elements
map< vector< int > , int > EM1 = dropTrivialEdgesInMap( modEdgeMap( N , T1 , getEdgeMap( N , w1 ) ) );
map< vector< int > , int > EM2 = dropTrivialEdgesInMap( modEdgeMap( N , T1 , getEdgeMap( N , w2 ) ) );
//----------------------------------------------------------
// G. For all possible x1-transformations:
int u1 = T1[0];
for( int i=0 ; i<u1 ; ++i ) {
// 1. Apply an x1-shift
EM1 = x1shiftEdgeMap( N , T1 , EM1 );
// 2. Suggest a tail-conjugator
pair< bool , vector< int > > r = suggestTailConjugator( N , EM1 , EM2 );
r.second[0] += i+1;
// 3. Test a tail conjugator
if( r.first ) {
if( testVector( N , T1 , w1 , w2 , r.second ) ) {
// Find an actual conjugator
Word C = getTailWord( N , r.second );
Word D = getLoopConjugator( N , T1 , w1 , -C*w2*C );
return pair< bool , Word >( true , M1.imageOf( M2.imageOf( D*-C ) ) );
}
}
}
return pair< bool , Word >( false , Word() );
}
//----------------------------------------------------------------
// Function: TopoDS_Shape level function to update the core Surface
// for any movement or Boolean operation of the body.
// Author: Jane Hu
//----------------------------------------------------------------
CubitStatus OCCSurface::update_OCC_entity(TopoDS_Face& old_surface,
TopoDS_Shape& new_surface,
BRepBuilderAPI_MakeShape *op,
TopoDS_Vertex* removed_vertex,
LocOpe_SplitShape* sp)
{
//set the Wires
TopTools_IndexedMapOfShape M, M2;
TopoDS_Shape shape, shape2, shape_edge, shape_vertex;
TopExp::MapShapes(old_surface, TopAbs_WIRE, M);
TopTools_ListOfShape shapes;
BRepFilletAPI_MakeFillet2d* test_op = NULL;
for (int ii=1; ii<=M.Extent(); ii++)
{
TopoDS_Wire wire = TopoDS::Wire(M(ii));
TopTools_ListOfShape shapes;
if(op)
{
test_op = dynamic_cast<BRepFilletAPI_MakeFillet2d*>(op);
if(!test_op)
shapes.Assign(op->Modified(wire));
if(shapes.Extent() == 0)
shapes.Assign(op->Generated(wire));
if(!new_surface.IsNull())
TopExp::MapShapes(new_surface,TopAbs_WIRE, M2);
}
else if(sp)
shapes.Assign(sp->DescendantShapes(wire));
if (shapes.Extent() == 1)
{
shape = shapes.First();
if(M2.Extent() == 1)
{
shape2 = TopoDS::Wire(M2(1));
if(!shape.IsSame(shape2))
shape = shape2;
}
else if(M2.Extent() > 1)
shape.Nullify();
}
else if(shapes.Extent() > 1)
shape.Nullify();
else if(op->IsDeleted(wire) || shapes.Extent() == 0)
{
TopTools_IndexedMapOfShape M_new;
TopExp::MapShapes(new_surface, TopAbs_WIRE, M_new);
if (M_new.Extent()== 1)
shape = M_new(1);
else
shape.Nullify();
}
else
{
shape = wire;
continue;
}
//set curves
BRepTools_WireExplorer Ex;
for(Ex.Init(wire); Ex.More();Ex.Next())
{
TopoDS_Edge edge = Ex.Current();
if(op && !test_op)
{
shapes.Assign(op->Modified(edge));
if(shapes.Extent() == 0)
shapes.Assign(op->Generated(edge));
}
else if(sp)
shapes.Assign(sp->DescendantShapes(edge));
if (shapes.Extent() == 1)
{
//in fillet creating mothod, one edge could generated a face, so check
//it here.
TopAbs_ShapeEnum type = shapes.First().TShape()->ShapeType();
if(type != TopAbs_EDGE)
shape_edge.Nullify();
else
shape_edge = shapes.First();
}
else if (shapes.Extent() > 1)
{
//update all attributes first.
TopTools_ListIteratorOfListOfShape it;
it.Initialize(shapes);
for(; it.More(); it.Next())
{
shape_edge = it.Value();
OCCQueryEngine::instance()->copy_attributes(edge, shape_edge);
}
shape_edge.Nullify();
}
else if (op->IsDeleted(edge))
shape_edge.Nullify();
else if (test_op)
{
if(!test_op->IsModified(edge))
shape_edge = edge;
else
shape_edge = (test_op->Modified(edge)).First();
}
else
shape_edge = edge;
//update vertex
TopoDS_Vertex vertex = Ex.CurrentVertex();
shapes.Clear();
if(test_op)
assert(removed_vertex != NULL);
if(op && ! test_op )
shapes.Assign(op->Modified(vertex));
else if(sp)
shapes.Assign(sp->DescendantShapes(vertex));
if (shapes.Extent() == 1)
shape_vertex = shapes.First();
else if(shapes.Extent() > 1)
{
//update all attributes first.
TopTools_ListIteratorOfListOfShape it;
it.Initialize(shapes);
for(; it.More(); it.Next())
{
shape_vertex = it.Value();
OCCQueryEngine::instance()->copy_attributes(vertex, shape_vertex);
}
shape_vertex.Nullify() ;
}
else if(op->IsDeleted(vertex) || (test_op && vertex.IsSame( *removed_vertex)))
shape_vertex.Nullify() ;
else
shape_vertex = vertex;
if(!vertex.IsSame(shape_vertex) )
OCCQueryEngine::instance()->update_OCC_map(vertex, shape_vertex);
if (!edge.IsSame(shape_edge))
OCCQueryEngine::instance()->update_OCC_map(edge, shape_edge);
}
if (!wire.IsSame(shape))
OCCQueryEngine::instance()->update_OCC_map(wire, shape);
}
double dTOL = OCCQueryEngine::instance()->get_sme_resabs_tolerance();
if (!old_surface.IsSame(new_surface))
{
TopAbs_ShapeEnum shapetype = TopAbs_SHAPE;
if(!new_surface.IsNull())
shapetype = new_surface.TShape()->ShapeType();
if(shapetype == TopAbs_FACE || new_surface.IsNull())
OCCQueryEngine::instance()->update_OCC_map(old_surface, new_surface);
else
{
TopTools_IndexedMapOfShape M;
TopExp::MapShapes(new_surface, TopAbs_FACE, M);
TopoDS_Shape new_shape;
if(M.Extent() == 1)
new_shape = M(1);
else if(M.Extent() > 1)
{
for(int i = 1; i <= M.Extent(); i++)
{
GProp_GProps myProps;
BRepGProp::SurfaceProperties(old_surface, myProps);
double orig_mass = myProps.Mass();
gp_Pnt orig_pnt = myProps.CentreOfMass();
BRepGProp::SurfaceProperties(M(i), myProps);
double after_mass = myProps.Mass();
gp_Pnt after_pnt = myProps.CentreOfMass();
if(fabs(-after_mass + orig_mass) <= dTOL &&
orig_pnt.IsEqual(after_pnt, dTOL))
{
new_shape = M(i);
break;
}
}
}
OCCQueryEngine::instance()->update_OCC_map(old_surface, new_shape);
}
}
return CUBIT_SUCCESS;
}
void TestMatrix(ostream& os)
{
// display a headline
os << "Matrix test\r\n===========\r\n";
Matrix<int> A(3,3), B(3,3), C(3,3), D(3,3);
A(0,0) = 1;
A(0,1) = 3;
A(0,2) = -4;
A(1,0) = 1;
A(1,1) = 1;
A(1,2) = -2;
A(2,0) = -1;
A(2,1) = -2;
A(2,2) = 5;
B(0,0) = 8;
B(0,1) = 3;
B(0,2) = 0;
B(1,0) = 3;
B(1,1) = 10;
B(1,2) = 2;
B(2,0) = 0;
B(2,1) = 2;
B(2,2) = 6;
D(0,0) = 1;
D(0,1) = 2;
D(0,2) = -1;
D(1,0) = 2;
D(1,1) = -1;
D(1,2) = -3;
D(2,0) = 0;
D(2,1) = -2;
D(2,2) = 4;
os << "\r\nMatrix A = \r\n";
ShowMatrix(os,A);
os << "\r\nMatrix B = \r\n";
ShowMatrix(os,B);
C = A % B;
os << "\r\nMatrix C (A % B) = \r\n";
ShowMatrix(os,C);
C = A + B;
os << "\r\nMatrix C (A + B) = \r\n";
ShowMatrix(os,C);
C = A;
C += B;
os << "\r\nMatrix C (= A, += B) =\r\n";
ShowMatrix(os,C);
C = A + 1;
os << "\r\nMatrix C (= A + 1) =\r\n";
ShowMatrix(os,C);
C += 1;
os << "\r\nMatrix C (+= 1) =\r\n";
ShowMatrix(os,C);
C = A - B;
os << "\r\nMatrix C (A - B) = \r\n";
ShowMatrix(os,C);
C = A;
C -= B;
os << "\r\nMatrix C (= A, -= B) =\r\n";
ShowMatrix(os,C);
C = A - 1;
os << "\r\nMatrix C (= A - 1) =\r\n";
ShowMatrix(os,C);
C -= 1;
os << "\r\nMatrix C (-= 1) =\r\n";
ShowMatrix(os,C);
C = A * B;
os << "\r\nMatrix C (A * B) = \r\n";
ShowMatrix(os,C);
C = A;
C *= B;
os << "\r\nMatrix C (= A, *= B) =\r\n";
ShowMatrix(os,C);
C = A * 2;
os << "\r\nMatrix C (= A * 2) =\r\n";
ShowMatrix(os,C);
C *= 2;
os << "\r\nMatrix C (*= 2) =\r\n";
ShowMatrix(os,C);
C = B / A;
os << "\r\nMatrix C (B / A) = \r\n";
ShowMatrix(os,C);
C = B;
C /= A;
os << "\r\nMatrix C (= B, /= A) =\r\n";
ShowMatrix(os,C);
C = A / 2;
os << "\r\nMatrix C (= A / 2) =\r\n";
ShowMatrix(os,C);
C /= 2;
os << "\r\nMatrix C (/= 2) =\r\n";
ShowMatrix(os,C);
C = -A;
os << "\r\nMatrix C (-A) = \r\n";
ShowMatrix(os,C);
// test comparisons
os << "\r\nMatrix A = \r\n";
ShowMatrix(os,A);
os << "\r\nMatrix D = \r\n";
ShowMatrix(os,D);
if (A.Equals(D))
os << "\r\nERROR: A should not equal D";
else
os << "\r\nOKAY: A not equal D";
C = A;
if (A.Equals(C))
os << "\r\nOKAY: A equals C\r\n";
else
os << "\r\nERROR: A should equal C\r\n";
Matrix<bool> I(3,3);
I = (A == D);
os << "\r\nMatrix I = (A == D)\r\n";
ShowMatrix(os,I);
I = (A != D);
os << "\r\nMatrix I = (A != D)\r\n";
ShowMatrix(os,I);
I = (A < D);
os << "\r\nMatrix I = (A < D)\r\n";
ShowMatrix(os,I);
I = (A <= D);
os << "\r\nMatrix I = (A <= D)\r\n";
ShowMatrix(os,I);
I = (A > D);
os << "\r\nMatrix I = (A > D)\r\n";
ShowMatrix(os,I);
I = (A >= D);
os << "\r\nMatrix I = (A >= D)\r\n";
ShowMatrix(os,I);
// check fill function
C.Fill(9);
os << "\r\nC filled with 9 =\r\n";
ShowMatrix(os,C);
// check Apply functions
C = Apply(A, Times2);
os << "\r\nC = A.Apply(Times2)\r\n";
ShowMatrix(os,C);
C.Apply(Times2);
os << "\r\nApply(C,Times2)\r\n";
ShowMatrix(os,C);
// check row and column vector functions
Matrix<int> S(1,1);
Matrix<int> r1A(3,1);
Matrix<int> c0B(1,3);
r1A = A.VectorRow(1);
c0B = B.VectorCol(0);
os << "\r\nMatrix S = \r\n";
ShowMatrix(os,S);
os << "\r\nMatrix R1A = \r\n";
ShowMatrix(os,r1A);
os << "\r\nMatrix C0B = \r\n";
ShowMatrix(os,c0B);
if (r1A.IsRowVector())
os << "\r\nOKAY: R1A is row vector";
else
os << "\r\nERROR: R1A should be a row vector";
if (!r1A.IsColVector())
os << "\r\nOKAY: R1A is not a column vector";
else
os << "\r\nERROR: R1A should not be a column vector";
if (!c0B.IsRowVector())
os << "\r\nOKAY: C0B is not a row vector";
else
os << "\r\nERROR: C0B should not be a row vector";
if (c0B.IsColVector())
os << "\r\nOKAY: C0B is column vector";
else
os << "\r\nERROR: C0B should be a column vector";
if (c0B.IsVector())
os << "\r\nOKAY: C0B is a vector";
else
os << "\r\nERROR: C0B should be a vector";
if (!A.IsVector())
os << "\r\nOKAY: A is not a vector";
else
os << "\r\nERROR: A should not be a vector";
if (!c0B.IsSquare())
os << "\r\nOKAY: C0B is not square";
else
os << "\r\nERROR: C0B should not be square";
if (A.IsSquare())
os << "\r\nOKAY: A is square";
else
os << "\r\nERROR: A should be square";
B.Fill(0);
if (B.IsZero())
os << "\r\nOKAY: B is zero";
else
os << "\r\nERROR: B should be zero";
if (!A.IsZero())
os << "\r\nOKAY: A is not zero";
else
os << "\r\nERROR: A should not be zero";
// test inner product
int ip = r1A.InnerProduct(c0B);
os << "\r\n\r\ninner product of R1A and C0B = " << ip << "\r\n";
// make some bigger matrices
Matrix<int> M1(5,5), M2(5,5,3), M3(5,5), M4(5,5);
const int junk[] = { 1, 5, 3, 0, 1,
0, 2, 0, 4, 5,
1, 0, 0, 2, 3,
7, 1, 3, 0, 0,
2, 1, 0, 4, 6 };
const int ident[] = { 1, 0, 0, 0, 0,
0, 1, 0, 0, 0,
0, 0, 1, 0, 0,
0, 0, 0, 1, 0,
0, 0, 0, 0, 1 };
const int tridi[] = { 1, 1, 0, 0, 0,
1, 1, 1, 0, 0,
0, 1, 1, 1, 0,
0, 0, 1, 1, 1,
0, 0, 0, 1, 1 };
const int utri[] = { 1, 1, 1, 1, 1,
0, 1, 1, 1, 1,
0, 0, 1, 1, 1,
0, 0, 0, 1, 1,
0, 0, 0, 0, 1 };
const int ltri[] = { 1, 0, 0, 0, 0,
1, 1, 0, 0, 0,
1, 1, 1, 0, 0,
1, 1, 1, 1, 0,
1, 1, 1, 1, 1 };
const int perm[] = { 0, 1, 0, 0, 0,
1, 0, 0, 0, 0,
0, 0, 0, 1, 0,
0, 0, 0, 0, 1,
0, 0, 1, 0, 0 };
const int det[] = { 3, 5, 3, 8, 1,
2, 6, 3, 4, 5,
1, 4, 5, 2, 3,
7, 1, 3, 6, 8,
2, 4, 1, 4, 9 };
M1 = ident;
M3 = M1 * 2;
M4 = junk;
os << "\r\nmatrix M1 = \r\n";
ShowMatrix(os,M1);
os << "\r\nmatrix M2 = \r\n";
ShowMatrix(os,M2);
os << "\r\nmatrix M3 = \r\n";
ShowMatrix(os,M3);
os << "\r\nmatrix M4 = \r\n";
ShowMatrix(os,M4);
if (M1.IsDiagonal())
os << "\r\nOKAY: M1 is diagonal";
else
os << "\r\nERROR: M1 should be diagonal";
if (M1.IsIdentity())
os << "\r\nOKAY: M1 is an identity matrix";
else
os << "\r\nERROR: M1 should be an identity matrix";
if (!M2.IsDiagonal())
os << "\r\nOKAY: M2 is not diagonal";
else
os << "\r\nERROR: M2 should not be diagonal";
if (!M2.IsIdentity())
os << "\r\nOKAY: M2 is not an identity matrix";
else
os << "\r\nERROR: M2 should not be an identity matrix";
if (M3.IsDiagonal())
os << "\r\nOKAY: M3 is diagonal";
else
os << "\r\nERROR: M3 should be diagonal";
if (!M3.IsIdentity())
os << "\r\nOKAY: M3 is not an identity matrix";
else
os << "\r\nERROR: M3 should not be an identity matrix";
if (!M4.IsDiagonal())
os << "\r\nOKAY: M4 is not diagonal";
else
os << "\r\nERROR: M4 should not be diagonal";
if (!M4.IsIdentity())
os << "\r\nOKAY: M4 is not an identity matrix";
else
os << "\r\nERROR: M4 should not be an identity matrix";
// tridiagonal tests
M1 = tridi;
os << "\r\n\r\nmatrix M1 = \r\n";
ShowMatrix(os,M1);
if (M1.IsTridiagonal())
os << "\r\nOKAY: M1 is tridiagonal";
else
os << "\r\nERROR: M1 should be tridiagonal";
if (!M4.IsTridiagonal())
os << "\r\nOKAY: M4 is not tridiagonal";
else
os << "\r\nERROR: M1 should not be tridiagonal";
// upper triangular tests
M1 = utri;
os << "\r\n\r\nmatrix M1 = \r\n";
ShowMatrix(os,M1);
if (M1.IsUpperTriangular())
os << "\r\nOKAY: M1 is upper-triangular";
else
os << "\r\nERROR: M1 should be upper-triangular";
if (!M4.IsUpperTriangular())
os << "\r\nOKAY: M4 is not upper-triangular";
else
os << "\r\nERROR: M4 should not be upper-triangular";
// lower triangular tests
M1 = ltri;
os << "\r\n\r\nmatrix M1 = \r\n";
ShowMatrix(os,M1);
if (M1.IsLowerTriangular())
os << "\r\nOKAY: M1 is lower-triangular";
else
os << "\r\nERROR: M1 should be lower-triangular";
if (!M4.IsLowerTriangular())
os << "\r\nOKAY: M4 is not lower-triangular";
else
os << "\r\nERROR: M4 should not be lower-triangular";
// permutation tests
M1 = perm;
os << "\r\n\r\nmatrix M1 = \r\n";
ShowMatrix(os,M1);
M2 = ident;
os << "\r\n\r\nmatrix M2 = \r\n";
ShowMatrix(os,M2);
if (M1.IsPermutation())
os << "\r\nOKAY: M1 is permutation matrix";
else
os << "\r\nERROR: M1 should be permutation";
if (M2.IsPermutation())
os << "\r\nOKAY: M2 is permutation matrix";
else
os << "\r\nERROR: M2 should be permutation";
if (!M4.IsPermutation())
os << "\r\nOKAY: M4 is not permutation";
else
os << "\r\nERROR: M4 should not be permutation";
// check singularity function
M1(0,1) = 0;
os << "\r\n\r\nmatrix M1 = \r\n";
ShowMatrix(os,M1);
if (M1.IsSingular())
os << "\r\nOKAY: M1 is singular";
else
os << "\r\nERROR: M1 should be singular";
if (!M2.IsSingular())
os << "\r\nOKAY: M2 is not singular";
else
os << "\r\nERROR: M2 should not be singular";
if (!M4.IsSingular())
os << "\r\nOKAY: M4 is not singular";
else
os << "\r\nERROR: M4 should not be singular";
// change main window heading
os <<endl
<<"Matrix Tests (manipulations)" <<endl
<<"============================" <<endl;
// test minors and determinants
os << "\r\n\r\nmatrix M4 = \r\n";
ShowMatrix(os,M4);
os << "\r\nminor M4(1,1) = \r\n";
ShowMatrix(os,M4.Minor(1,1));
os << "\r\nminor M4(0,4) = \r\n";
ShowMatrix(os,M4.Minor(0,4));
Matrix<int> M5(2,2), M6(3,3);
M5(0,0) = 1;
M5(0,1) = 2;
M5(1,0) = 3;
M5(1,1) = 4;
M6(0,0) = 1;
M6(0,1) = 3;
M6(0,2) = 2;
M6(1,0) = 5;
M6(1,1) = 4;
M6(1,2) = 7;
M6(2,0) = 6;
M6(2,1) = 9;
M6(2,2) = 8;
M4 = det;
Matrix<int> T4(5,5), T5(2,2), T6(3,3);
T4 = M4.Transpose();
T5 = M5.Transpose();
T6 = M6.Transpose();
os << "\r\nmatrix M5 = \r\n";
ShowMatrix(os,M5);
os << "\r\ndeterminant of M5 = "
<< M5.Determinant() << "\r\n";
os << "\r\nmatrix T5 = \r\n";
ShowMatrix(os,T5);
os << "\r\ndeterminant of T5 = "
<< T5.Determinant() << "\r\n";
os << "\r\nmatrix M6 = \r\n";
ShowMatrix(os,M6);
os << "\r\ndeterminant of M6 = "
<< M6.Determinant() << "\r\n";
os << "\r\nmatrix T6 = \r\n";
ShowMatrix(os,T6);
os << "\r\ndeterminant of T6 = "
<< T6.Determinant() << "\r\n";
os << "\r\nmatrix M4 = \r\n";
ShowMatrix(os,M4);
os << "\r\ndeterminant of M4 = "
<< M4.Determinant() << "\r\n";
os << "\r\nmatrix T4 = \r\n";
ShowMatrix(os,T4);
os << "\r\ndeterminant of T4 = "
<< T4.Determinant() << "\r\n";
Matrix<int> R;
os << "\r\nMatrix R (def. constr.) = \r\n";
ShowMatrix(os,R);
R.Resize(10,10);
os << "\r\nMatrix R (now 10x10) = \r\n";
ShowMatrix(os,R);
// change main window heading
os <<endl
<<"Matrix Tests (double)" <<endl
<<"=====================" <<endl;
// check <double> Matrix
os << "\r\nFLOATING POINT!";
Matrix<double> X(3,4), Y(4,3), Z(3,3);
X(0,0) = 1.0; X(1,0) = 5.0; X(2,0) = 2.0;
X(0,1) = 2.0; X(1,1) = 2.0; X(2,1) = 4.0;
X(0,2) = 0.0; X(1,2) = 3.0; X(2,2) = 3.0;
X(0,3) = 1.0; X(1,3) = 2.0; X(2,3) = 1.0;
Y(0,0) = 0.0; Y(2,0) = 1.0;
Y(0,1) = 1.0; Y(2,1) = 0.0;
Y(0,2) = 2.0; Y(2,2) = 5.0;
Y(1,0) = 1.0; Y(3,0) = 3.0;
Y(1,1) = 3.0; Y(3,1) = 1.0;
Y(1,2) = 2.0; Y(3,2) = 2.0;
os << "\r\nMatrix X = \r\n";
ShowMatrix(os,X);
os << "\r\nMatrix Y = \r\n";
ShowMatrix(os,Y);
Z = X % Y;
os << "\r\nMatrix Z (X % Y) = \r\n";
ShowMatrix(os,Z);
// check transposition
Matrix<double> tX;
tX = X.Transpose();
os << "\r\nOriginal X =\r\n";
ShowMatrix(os,X);
os << "\r\nTranspose X =\r\n";
ShowMatrix(os,tX);
X(0,0) = 1; X(0,1) = 3; X(0,2) = -4; X(0,3) = 8;
X(1,0) = 1; X(1,1) = 1; X(1,2) = -2; X(1,3) = 2;
X(2,0) = -1; X(2,1) = -2; X(2,2) = 5; X(2,3) = -1;
os << "\r\nOriginal X =\r\n";
ShowMatrix(os,X);
Matrix<double> lX(X.LinSolve());
os << "\r\nX after elimination =\r\n";
ShowMatrix(os,X);
os << "\r\nlinear equation solution =\r\n";
ShowMatrix(os,lX);
X(0,0) = 1.0; X(1,0) = 3.0; X(2,0) = 5.0;
X(0,1) = 2.0; X(1,1) = 5.0; X(2,1) = 6.0;
X(0,2) = 0.0; X(1,2) = 4.0; X(2,2) = 3.0;
X(0,3) = 0.1; X(1,3) = 12.5; X(2,3) = 10.3;
os << "\r\nOriginal X =\r\n";
ShowMatrix(os,X);
lX = X.LinSolve();
os << "\r\nX after elimination =\r\n";
ShowMatrix(os,X);
os << "\r\nlinear equation solution =\r\n";
ShowMatrix(os,lX);
Matrix<double> Adbl(3,3), Bdbl(3,1);
Adbl(0,0) = 1.0; Adbl(0,1) = 2.0; Adbl(0,2) = 0.0;
Adbl(1,0) = 3.0; Adbl(1,1) = 5.0; Adbl(1,2) = 4.0;
Adbl(2,0) = 5.0; Adbl(2,1) = 6.0; Adbl(2,2) = 3.0;
Bdbl(0,0) = 0.1;
Bdbl(1,0) = 12.5;
Bdbl(2,0) = 10.3;
os << "\r\n\r\nmatrix Adbl = \r\n";
ShowMatrix(os,Adbl);
os << "\r\nmatrix Bdbl = \r\n";
ShowMatrix(os,Bdbl);
Matrix<double> alup(Adbl); // copy Adbl before LUP decomp
os << "\r\nLU decomp of Adbl (before) = \r\n";
ShowMatrix(os,alup);
Matrix<size_t> aperm = alup.LUPDecompose();
os << "\r\nLU decomp of Adbl (after) = \r\n";
ShowMatrix(os,alup);
os << "\r\nPermutation of Adbl = \r\n";
ShowMatrix(os,aperm);
Matrix<double> asol = alup.LUPSolve(aperm,Bdbl);
os << "\r\nlinear solution of Adbl and Bdbl = \r\n";
ShowMatrix(os,asol);
Matrix<double> ainv = alup.LUPInvert(aperm);
os << "\r\ninverse of Adbl and Bdbl = \r\n";
ShowMatrix(os,ainv);
Matrix<double> aid = Adbl % ainv;
os << "\r\ninverse dot Adbl = \r\n";
ShowMatrix(os,aid);
Grid<size_t> iperm = ainv.LUPDecompose();
Matrix<double> invinv = ainv.LUPInvert(iperm);
os << "\r\ninverse of inverse =\r\n";
ShowMatrix(os,invinv);
}
int
main()
{
try {
int err = 1;
{
vpColVector c(6, 1);
vpRowVector r(6, 1);
std::vector<double> bench(6, 1);
vpMatrix M1(c);
if (test("M1", M1, bench) == false)
return err;
vpMatrix M2(r);
if (test("M2", M2, bench) == false)
return err;
}
{
vpMatrix M(4,5);
int val = 0;
for(unsigned int i=0; i<M.getRows(); i++) {
for(unsigned int j=0; j<M.getCols(); j++) {
M[i][j] = val++;
}
}
std::cout <<"M ";
M.print (std::cout, 4);
vpMatrix N;
N.init(M, 0, 1, 2, 3);
std::cout <<"N ";
N.print (std::cout, 4);
std::string header("My 4-by-5 matrix\nwith a second line");
// Save matrix in text format
if (vpMatrix::saveMatrix("matrix.mat", M, false, header.c_str()))
std::cout << "Matrix saved in matrix.mat file" << std::endl;
else
return err;
// Load matrix in text format
vpMatrix M1;
char header_[100];
if (vpMatrix::loadMatrix("matrix.mat", M1, false, header_))
std::cout << "Matrix loaded from matrix.mat file with header \"" << header_ << "\": \n" << M1 << std::endl;
else
return err;
if (header != std::string(header_)) {
std::cout << "Bad header in matrix.mat" << std::endl;
return err;
}
// Save matrix in binary format
if (vpMatrix::saveMatrix("matrix.bin", M, true, header.c_str()))
std::cout << "Matrix saved in matrix.bin file" << std::endl;
else
return err;
// Load matrix in binary format
if (vpMatrix::loadMatrix("matrix.bin", M1, true, header_))
std::cout << "Matrix loaded from matrix.bin file with header \"" << header_ << "\": \n" << M1 << std::endl;
else
return err;
if (header != std::string(header_)) {
std::cout << "Bad header in matrix.bin" << std::endl;
return err;
}
// Save matrix in YAML format
if (vpMatrix::saveMatrixYAML("matrix.yml", M, header.c_str()))
std::cout << "Matrix saved in matrix.yml file" << std::endl;
else
return err;
// Read matrix in YAML format
vpMatrix M2;
if (vpMatrix::loadMatrixYAML("matrix.yml", M2, header_))
std::cout << "Matrix loaded from matrix.yml file with header \"" << header_ << "\": \n" << M2 << std::endl;
else
return err;
if (header != std::string(header_)) {
std::cout << "Bad header in matrix.mat" << std::endl;
return err;
}
}
{
vpRotationMatrix R(vpMath::rad(10), vpMath::rad(20), vpMath::rad(30));
std::cout << "R: \n" << R << std::endl;
vpMatrix M1(R);
std::cout << "M1: \n" << M1 << std::endl;
vpMatrix M2(M1);
std::cout << "M2: \n" << M2 << std::endl;
vpMatrix M3 = R;
std::cout << "M3: \n" << M3 << std::endl;
vpMatrix M4 = M1;
std::cout << "M4: \n" << M4 << std::endl;
}
{
std::cout << "------------------------" << std::endl;
std::cout << "--- TEST PRETTY PRINT---" << std::endl;
std::cout << "------------------------" << std::endl;
vpMatrix M ;
M.eye(4);
std::cout << "call std::cout << M;" << std::endl;
std::cout << M << std::endl;
std::cout << "call M.print (std::cout, 4);" << std::endl;
M.print (std::cout, 4);
std::cout << "------------------------" << std::endl;
M.resize(3,3) ;
M.eye(3);
M[1][0]=1.235;
M[1][1]=12.345;
M[1][2]=.12345;
std::cout << "call std::cout << M;" << std::endl;
std::cout << M;
std::cout << "call M.print (std::cout, 6);" << std::endl;
M.print (std::cout, 6);
std::cout << std::endl;
std::cout << "------------------------" << std::endl;
M[0][0]=-1.235;
M[1][0]=-12.235;
std::cout << "call std::cout << M;" << std::endl;
std::cout << M << std::endl;
std::cout << "call M.print (std::cout, 10);" << std::endl;
M.print (std::cout, 10);
std::cout << std::endl;
std::cout << "call M.print (std::cout, 2);" << std::endl;
M.print (std::cout, 2);
std::cout << std::endl;
std::cout << "------------------------" << std::endl;
M.resize(3,3) ;
M.eye(3);
M[0][2]=-0.0000000876;
std::cout << "call std::cout << M;" << std::endl;
std::cout << M << std::endl;
std::cout << "call M.print (std::cout, 4);" << std::endl;
M.print (std::cout, 4);
std::cout << std::endl;
std::cout << "call M.print (std::cout, 10, \"M\");" << std::endl;
M.print (std::cout, 10, "M");
std::cout << std::endl;
std::cout << "call M.print (std::cout, 20, \"M\");" << std::endl;
M.print (std::cout, 20, "M");
std::cout << std::endl;
std::cout << "------------------------" << std::endl;
std::cout << "--- TEST RESIZE --------" << std::endl;
std::cout << "------------------------" << std::endl;
std::cout << "5x5" << std::endl;
M.resize(5,5,false);
std::cout << M << std::endl;
std::cout << "3x2" << std::endl;
M.resize(3,2,false);
std::cout << M << std::endl;
std::cout << "2x2" << std::endl;
M.resize(2,2,false);
std::cout << M << std::endl;
std::cout << "------------------------" << std::endl;
vpVelocityTwistMatrix vMe;
vpMatrix A(1,6),B;
A=1.0;
//vMe=1.0;
B=A*vMe;
std::cout << "------------------------" << std::endl;
std::cout << "--- TEST vpRowVector * vpColVector" << std::endl;
std::cout << "------------------------" << std::endl;
vpRowVector r(3);
r[0] = 2;
r[1] = 3;
r[2] = 4;
vpColVector c(3);
c[0] = 1;
c[1] = 2;
c[2] = -1;
double rc = r * c;
r.print(std::cout, 2, "r");
c.print(std::cout, 2, "c");
std::cout << "r * c = " << rc << std::endl;
std::cout << "------------------------" << std::endl;
std::cout << "--- TEST vpRowVector * vpMatrix" << std::endl;
std::cout << "------------------------" << std::endl;
M.resize(3,3) ;
M.eye(3);
M[1][0] = 1.5;
M[2][0] = 2.3;
vpRowVector rM = r * M;
r.print(std::cout, 2, "r");
M.print(std::cout, 10, "M");
std::cout << "r * M = " << rM << std::endl;
std::cout << "------------------------" << std::endl;
std::cout << "--- TEST vpGEMM " << std::endl;
std::cout << "------------------------" << std::endl;
M.resize(3,3) ;
M.eye(3);
vpMatrix N(3, 3);
N[0][0] = 2;
N[1][0] = 1.2;
N[1][2] = 0.6;
N[2][2] = 0.25;
vpMatrix C(3, 3);
C.eye(3);
vpMatrix D;
//realise the operation D = 2 * M^T * N + 3 C
vpGEMM(M, N, 2, C, 3, D, VP_GEMM_A_T);
std::cout << D << std::endl;
std::cout << "All tests succeed" << std::endl;
return 0;
}
}
catch(vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
unsigned
vcl_fixed_token(const char *p, const char **q)
{
switch (p[0]) {
case '!':
M2('=', T_NEQ);
M2('~', T_NOMATCH);
M1();
case '%':
M1();
case '&':
M2('&', T_CAND);
M1();
case '(':
M1();
case ')':
M1();
case '*':
M2('=', T_MUL);
M1();
case '+':
M2('+', T_INC);
M2('=', T_INCR);
M1();
case ',':
M1();
case '-':
M2('-', T_DEC);
M2('=', T_DECR);
M1();
case '.':
M1();
case '/':
M2('=', T_DIV);
M1();
case ';':
M1();
case '<':
M2('<', T_SHL);
M2('=', T_LEQ);
M1();
case '=':
M2('=', T_EQ);
M1();
case '>':
M2('=', T_GEQ);
M2('>', T_SHR);
M1();
case 'e':
if (p[1] == 'l' && p[2] == 's' && p[3] == 'e' &&
p[4] == 'i' && p[5] == 'f' && !isvar(p[6])) {
*q = p + 6;
return (T_ELSEIF);
}
if (p[1] == 'l' && p[2] == 's' && p[3] == 'i' &&
p[4] == 'f' && !isvar(p[5])) {
*q = p + 5;
return (T_ELSIF);
}
if (p[1] == 'l' && p[2] == 's' && p[3] == 'e' &&
!isvar(p[4])) {
*q = p + 4;
return (T_ELSE);
}
return (0);
case 'i':
if (p[1] == 'n' && p[2] == 'c' && p[3] == 'l' &&
p[4] == 'u' && p[5] == 'd' && p[6] == 'e' &&
!isvar(p[7])) {
*q = p + 7;
return (T_INCLUDE);
}
M2('f', T_IF);
return (0);
case '{':
M1();
case '|':
M2('|', T_COR);
M1();
case '}':
M1();
case '~':
M1();
default:
return (0);
}
}
SEXP wink_both_par( SEXP data1, SEXP data2, SEXP windows, SEXP Rdelta, SEXP RB, SEXP Rnt) throw()
{
parameters param;
if( !param.load(data1,data2,windows,Rdelta,RB) )
return R_NilValue;
//==========================================================================
// Get num_threads
//==========================================================================
Rnt = coerceVector(Rnt,INTSXP);
size_t num_threads = INTEGER(Rnt)[0];
Rprintf("\t#### num_threads=%u\n",unsigned(num_threads));
if(num_threads<=0)
num_threads = 1;
try
{
//======================================================================
// transform R data intro C++ objects
//======================================================================
wink::c_matrix M1(param.nrow1,param.ncol1);
wink::c_matrix M2(param.nrow2,param.ncol2);
M1.loadR( REAL(data1) );
M2.loadR( REAL(data2) );
//======================================================================
// create the return vector
//======================================================================
SEXP Rval;
PROTECT(Rval = allocMatrix(REALSXP,2,param.num_windows) );
double *pvalues = REAL(Rval);
//======================================================================
// create the team of threads, starting ASAP
//======================================================================
wink::workers team(M1,
M2,
param.B,
num_threads,
param.num_windows,
param.ptr_windows,
pvalues,
param.delta,
&wink::worker::compute_pvalues_both);
//======================================================================
// wait for threads to complete
//======================================================================
team.wait();
UNPROTECT(1);
return Rval;
}
catch(...)
{
Rprintf("Exception in wink_both_par");
}
return R_NilValue;
}
void FastMatmulRecursive(LockAndCounter& locker, MemoryManager<Scalar>& mem_mngr, Matrix<Scalar>& A, Matrix<Scalar>& B, Matrix<Scalar>& C, int total_steps, int steps_left, int start_index, double x, int num_threads, Scalar beta) {
// Update multipliers
C.UpdateMultiplier(A.multiplier());
C.UpdateMultiplier(B.multiplier());
A.set_multiplier(Scalar(1.0));
B.set_multiplier(Scalar(1.0));
// Base case for recursion
if (steps_left == 0) {
MatMul(A, B, C);
return;
}
Matrix<Scalar> A11 = A.Subblock(2, 2, 1, 1);
Matrix<Scalar> A12 = A.Subblock(2, 2, 1, 2);
Matrix<Scalar> A21 = A.Subblock(2, 2, 2, 1);
Matrix<Scalar> A22 = A.Subblock(2, 2, 2, 2);
Matrix<Scalar> B11 = B.Subblock(2, 2, 1, 1);
Matrix<Scalar> B12 = B.Subblock(2, 2, 1, 2);
Matrix<Scalar> B21 = B.Subblock(2, 2, 2, 1);
Matrix<Scalar> B22 = B.Subblock(2, 2, 2, 2);
Matrix<Scalar> C11 = C.Subblock(2, 2, 1, 1);
Matrix<Scalar> C12 = C.Subblock(2, 2, 1, 2);
Matrix<Scalar> C21 = C.Subblock(2, 2, 2, 1);
Matrix<Scalar> C22 = C.Subblock(2, 2, 2, 2);
// Matrices to store the results of multiplications.
#ifdef _PARALLEL_
Matrix<Scalar> M1(mem_mngr.GetMem(start_index, 1, total_steps - steps_left, M), C11.m(), C11.m(), C11.n(), C.multiplier());
Matrix<Scalar> M2(mem_mngr.GetMem(start_index, 2, total_steps - steps_left, M), C11.m(), C11.m(), C11.n(), C.multiplier());
Matrix<Scalar> M3(mem_mngr.GetMem(start_index, 3, total_steps - steps_left, M), C11.m(), C11.m(), C11.n(), C.multiplier());
Matrix<Scalar> M4(mem_mngr.GetMem(start_index, 4, total_steps - steps_left, M), C11.m(), C11.m(), C11.n(), C.multiplier());
Matrix<Scalar> M5(mem_mngr.GetMem(start_index, 5, total_steps - steps_left, M), C11.m(), C11.m(), C11.n(), C.multiplier());
Matrix<Scalar> M6(mem_mngr.GetMem(start_index, 6, total_steps - steps_left, M), C11.m(), C11.m(), C11.n(), C.multiplier());
Matrix<Scalar> M7(mem_mngr.GetMem(start_index, 7, total_steps - steps_left, M), C11.m(), C11.m(), C11.n(), C.multiplier());
Matrix<Scalar> M8(mem_mngr.GetMem(start_index, 8, total_steps - steps_left, M), C11.m(), C11.m(), C11.n(), C.multiplier());
#else
Matrix<Scalar> M1(C11.m(), C11.n(), C.multiplier());
Matrix<Scalar> M2(C11.m(), C11.n(), C.multiplier());
Matrix<Scalar> M3(C11.m(), C11.n(), C.multiplier());
Matrix<Scalar> M4(C11.m(), C11.n(), C.multiplier());
Matrix<Scalar> M5(C11.m(), C11.n(), C.multiplier());
Matrix<Scalar> M6(C11.m(), C11.n(), C.multiplier());
Matrix<Scalar> M7(C11.m(), C11.n(), C.multiplier());
Matrix<Scalar> M8(C11.m(), C11.n(), C.multiplier());
#endif
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
bool sequential1 = should_launch_task(8, total_steps, steps_left, start_index, 1, num_threads);
bool sequential2 = should_launch_task(8, total_steps, steps_left, start_index, 2, num_threads);
bool sequential3 = should_launch_task(8, total_steps, steps_left, start_index, 3, num_threads);
bool sequential4 = should_launch_task(8, total_steps, steps_left, start_index, 4, num_threads);
bool sequential5 = should_launch_task(8, total_steps, steps_left, start_index, 5, num_threads);
bool sequential6 = should_launch_task(8, total_steps, steps_left, start_index, 6, num_threads);
bool sequential7 = should_launch_task(8, total_steps, steps_left, start_index, 7, num_threads);
bool sequential8 = should_launch_task(8, total_steps, steps_left, start_index, 8, num_threads);
#else
bool sequential1 = false;
bool sequential2 = false;
bool sequential3 = false;
bool sequential4 = false;
bool sequential5 = false;
bool sequential6 = false;
bool sequential7 = false;
bool sequential8 = false;
#endif
// M1 = (1 * A11) * (1 * B11)
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
# pragma omp task if(sequential1) shared(mem_mngr, locker) untied
{
#endif
M1.UpdateMultiplier(Scalar(1));
M1.UpdateMultiplier(Scalar(1));
FastMatmulRecursive(locker, mem_mngr, A11, B11, M1, total_steps, steps_left - 1, (start_index + 1 - 1) * 8, x, num_threads, Scalar(0.0));
#ifndef _PARALLEL_
#endif
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
locker.Decrement();
}
if (should_task_wait(8, total_steps, steps_left, start_index, 1, num_threads)) {
# pragma omp taskwait
# if defined(_PARALLEL_) && (_PARALLEL_ == _HYBRID_PAR_)
SwitchToDFS(locker, num_threads);
# endif
}
#endif
// M2 = (1 * A12) * (1 * B21)
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
# pragma omp task if(sequential2) shared(mem_mngr, locker) untied
{
#endif
M2.UpdateMultiplier(Scalar(1));
M2.UpdateMultiplier(Scalar(1));
FastMatmulRecursive(locker, mem_mngr, A12, B21, M2, total_steps, steps_left - 1, (start_index + 2 - 1) * 8, x, num_threads, Scalar(0.0));
#ifndef _PARALLEL_
#endif
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
locker.Decrement();
}
if (should_task_wait(8, total_steps, steps_left, start_index, 2, num_threads)) {
# pragma omp taskwait
# if defined(_PARALLEL_) && (_PARALLEL_ == _HYBRID_PAR_)
SwitchToDFS(locker, num_threads);
# endif
}
#endif
// M3 = (1 * A11) * (1 * B12)
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
# pragma omp task if(sequential3) shared(mem_mngr, locker) untied
{
#endif
M3.UpdateMultiplier(Scalar(1));
M3.UpdateMultiplier(Scalar(1));
FastMatmulRecursive(locker, mem_mngr, A11, B12, M3, total_steps, steps_left - 1, (start_index + 3 - 1) * 8, x, num_threads, Scalar(0.0));
#ifndef _PARALLEL_
#endif
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
locker.Decrement();
}
if (should_task_wait(8, total_steps, steps_left, start_index, 3, num_threads)) {
# pragma omp taskwait
# if defined(_PARALLEL_) && (_PARALLEL_ == _HYBRID_PAR_)
SwitchToDFS(locker, num_threads);
# endif
}
#endif
// M4 = (1 * A12) * (1 * B22)
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
# pragma omp task if(sequential4) shared(mem_mngr, locker) untied
{
#endif
M4.UpdateMultiplier(Scalar(1));
M4.UpdateMultiplier(Scalar(1));
FastMatmulRecursive(locker, mem_mngr, A12, B22, M4, total_steps, steps_left - 1, (start_index + 4 - 1) * 8, x, num_threads, Scalar(0.0));
#ifndef _PARALLEL_
#endif
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
locker.Decrement();
}
if (should_task_wait(8, total_steps, steps_left, start_index, 4, num_threads)) {
# pragma omp taskwait
# if defined(_PARALLEL_) && (_PARALLEL_ == _HYBRID_PAR_)
SwitchToDFS(locker, num_threads);
# endif
}
#endif
// M5 = (1 * A21) * (1 * B11)
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
# pragma omp task if(sequential5) shared(mem_mngr, locker) untied
{
#endif
M5.UpdateMultiplier(Scalar(1));
M5.UpdateMultiplier(Scalar(1));
FastMatmulRecursive(locker, mem_mngr, A21, B11, M5, total_steps, steps_left - 1, (start_index + 5 - 1) * 8, x, num_threads, Scalar(0.0));
#ifndef _PARALLEL_
#endif
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
locker.Decrement();
}
if (should_task_wait(8, total_steps, steps_left, start_index, 5, num_threads)) {
# pragma omp taskwait
# if defined(_PARALLEL_) && (_PARALLEL_ == _HYBRID_PAR_)
SwitchToDFS(locker, num_threads);
# endif
}
#endif
// M6 = (1 * A22) * (1 * B21)
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
# pragma omp task if(sequential6) shared(mem_mngr, locker) untied
{
#endif
M6.UpdateMultiplier(Scalar(1));
M6.UpdateMultiplier(Scalar(1));
FastMatmulRecursive(locker, mem_mngr, A22, B21, M6, total_steps, steps_left - 1, (start_index + 6 - 1) * 8, x, num_threads, Scalar(0.0));
#ifndef _PARALLEL_
#endif
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
locker.Decrement();
}
if (should_task_wait(8, total_steps, steps_left, start_index, 6, num_threads)) {
# pragma omp taskwait
# if defined(_PARALLEL_) && (_PARALLEL_ == _HYBRID_PAR_)
SwitchToDFS(locker, num_threads);
# endif
}
#endif
// M7 = (1 * A21) * (1 * B12)
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
# pragma omp task if(sequential7) shared(mem_mngr, locker) untied
{
#endif
M7.UpdateMultiplier(Scalar(1));
M7.UpdateMultiplier(Scalar(1));
FastMatmulRecursive(locker, mem_mngr, A21, B12, M7, total_steps, steps_left - 1, (start_index + 7 - 1) * 8, x, num_threads, Scalar(0.0));
#ifndef _PARALLEL_
#endif
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
locker.Decrement();
}
if (should_task_wait(8, total_steps, steps_left, start_index, 7, num_threads)) {
# pragma omp taskwait
# if defined(_PARALLEL_) && (_PARALLEL_ == _HYBRID_PAR_)
SwitchToDFS(locker, num_threads);
# endif
}
#endif
// M8 = (1 * A22) * (1 * B22)
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
# pragma omp task if(sequential8) shared(mem_mngr, locker) untied
{
#endif
M8.UpdateMultiplier(Scalar(1));
M8.UpdateMultiplier(Scalar(1));
FastMatmulRecursive(locker, mem_mngr, A22, B22, M8, total_steps, steps_left - 1, (start_index + 8 - 1) * 8, x, num_threads, Scalar(0.0));
#ifndef _PARALLEL_
#endif
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
locker.Decrement();
}
if (should_task_wait(8, total_steps, steps_left, start_index, 8, num_threads)) {
# pragma omp taskwait
}
#endif
M_Add1(M1, M2, C11, x, false, beta);
M_Add2(M3, M4, C12, x, false, beta);
M_Add3(M5, M6, C21, x, false, beta);
M_Add4(M7, M8, C22, x, false, beta);
// Handle edge cases with dynamic peeling
#if defined(_PARALLEL_) && (_PARALLEL_ == _BFS_PAR_ || _PARALLEL_ == _HYBRID_PAR_)
if (total_steps == steps_left) {
mkl_set_num_threads_local(num_threads);
mkl_set_dynamic(0);
}
#endif
DynamicPeeling(A, B, C, 2, 2, 2, beta);
}
Func ColorMgather(Func stBasis, float angle, uint8_t * orders, Expr filterthreshold, Expr divisionthreshold, Expr divisionthreshold2) {
uint8_t x_order = orders[0];
uint8_t y_order = orders[1];
uint8_t t_order = orders[2];
uint8_t c_order = orders[3];
Func X("X"),Y("Y"),T("T"),Xrg("Xrg"),Yrg("Yrg"),Trg("Trg");
uint8_t max_order = x_order;
// std::vector<Expr>Xk_expr (max_order,cast<float>(0.0f));
// std::vector<Expr>Yk_expr (max_order,cast<float>(0.0f));
// std::vector<Expr>Tk_expr (max_order,cast<float>(0.0f));
uint8_t Xk_uI[max_order];
uint8_t Yk_uI[max_order];
uint8_t Tk_uI[max_order];
Func Xk[max_order]; Func Yk[max_order]; Func Tk[max_order];
// Expr Xk[max_order],Yk[max_order],Tk[max_order];
for (int iO=0; iO < x_order; iO++) {
Xk[iO](x,y,t) = Expr(0.0f);
Yk[iO](x,y,t) = Expr(0.0f);
Tk[iO](x,y,t) = Expr(0.0f);
Xk_uI[iO] = 0;
Yk_uI[iO] = 0;
Tk_uI[iO] = 0;
}
int k = 0;
for (int iXo = 0; iXo < x_order; iXo++) // x_order
for (int iYo = 0; iYo < y_order; iYo++) // y_oder
for (int iTo = 0; iTo < t_order; iTo++) // t_order
for (int iCo = 0; iCo < c_order; iCo ++ ) // c_order: index of color channel
{
if ((iYo+iTo+iCo == 0 || iYo+iTo+iCo == 1)
&& ((iXo+iYo+iTo+iCo+1) < (x_order + 1))) {
X = ColorMgetfilter(stBasis, angle, iXo+1, iYo, iTo, iCo);
Y = ColorMgetfilter(stBasis, angle, iXo, iYo+1, iTo, iCo);
T = ColorMgetfilter(stBasis, angle, iXo, iYo, iTo+1, iCo);
Xrg = ColorMgetfilter(stBasis, angle, iXo+1, iYo, iTo, iCo+1);
Yrg = ColorMgetfilter(stBasis, angle, iXo, iYo+1, iTo, iCo+1);
Trg = ColorMgetfilter(stBasis, angle, iXo, iYo, iTo+1, iCo+1);
k = iXo + iYo + iTo + iCo;
Xk[k](x,y,t) += X(x,y,t) + Xrg(x,y,t);
Yk[k](x,y,t) += Y(x,y,t) + Yrg(x,y,t);
Tk[k](x,y,t) += T(x,y,t) + Trg(x,y,t);
Xk[k].update(Xk_uI[k]); Xk_uI[k]++;
Yk[k].update(Yk_uI[k]); Yk_uI[k]++;
Tk[k].update(Tk_uI[k]); Tk_uI[k]++;
}
}
// Scheduling
for (int iO = 0; iO <= k; iO++) {
Xk[iO].compute_root();
Yk[iO].compute_root();
Tk[iO].compute_root();
}
std::vector<Expr> st_expr(6,cast<float>(0.0f));
for (int iK=0; iK <= k; iK++) {
st_expr[0] += Xk[iK](x,y,t)*Tk[iK](x,y,t);
st_expr[1] += Tk[iK](x,y,t)*Tk[iK](x,y,t);
st_expr[2] += Xk[iK](x,y,t)*Xk[iK](x,y,t);
st_expr[3] += Yk[iK](x,y,t)*Tk[iK](x,y,t);
st_expr[4] += Yk[iK](x,y,t)*Yk[iK](x,y,t);
st_expr[5] += Xk[iK](x,y,t)*Yk[iK](x,y,t);
}
Func st("st"); st(x,y,t) = Tuple(st_expr);
st.compute_root();
Expr x_clamped = clamp(x,0,width-1);
Expr y_clamped = clamp(y,0,height-1);
Func st_clamped("st_clamped"); st_clamped(x,y,t) = st(x_clamped,y_clamped,t);
// float win = 7.0;
// Image<float> meanfilter(7,7,"meanfilter_data");
// meanfilter(x,y) = Expr(1.0f/(win*win));
// RDom rMF(meanfilter);
uint8_t win = 7;
RDom rMF(0,win,0,win);
Func st_filtered[6];
for (uint8_t iPc=0; iPc<6; iPc++) {
// iPc: index of product component
// Apply average filter
st_filtered[iPc](x,y,t) = sum(rMF,st_clamped(x + rMF.x,y + rMF.y,t)[iPc]/Expr(float(win*win)),"mean_filter");
st_filtered[iPc].compute_root();
}
// Tuple st_tuple = Tuple(st_expr);
// 4 debug
// Func tmpOut("tmpOut"); tmpOut(x,y,t) = Tuple(st_filtered[0](x,y,t),st_filtered[1](x,y,t),st_filtered[2](x,y,t),st_filtered[3](x,y,t),st_filtered[4](x,y,t),st_filtered[5](x,y,t));
// return tmpOut;
Tuple pbx = Tuple(st_filtered[2](x,y,t),st_filtered[5](x,y,t),st_filtered[0](x,y,t));
Tuple pby = Tuple(st_filtered[5](x,y,t),st_filtered[4](x,y,t),st_filtered[3](x,y,t));
Tuple pbt = Tuple(st_filtered[0](x,y,t),st_filtered[3](x,y,t),st_filtered[1](x,y,t));
Func pbxy("pbxy"); pbxy = cross(pby,pbx); pbxy.compute_root();
Func pbxt("pbxt"); pbxt = cross(pbx,pbt); pbxt.compute_root();
Func pbyt("pbyt"); pbyt = cross(pby,pbt); pbyt.compute_root();
Func pbxyd("pbxyd"); pbxyd = dot(pby,pbx); pbxyd.compute_root();
Func pbxtd("pbxtd"); pbxtd = dot(pbx,pbt); pbxtd.compute_root();
Func pbytd("pbytd"); pbytd = dot(pby,pbt); pbytd.compute_root();
// 4 debug
// Func tmpOut("tmpOut"); tmpOut(x,y,t) = Tuple(pbxy(x,y,t)[0],pbxt(x,y,t)[0],pbyt(x,y,t)[0],pbxyd(x,y,t),pbxtd(x,y,t),pbytd(x,y,t));
// return tmpOut;
Func yt_xy("yt_xy"); yt_xy = dot(pbyt(x,y,t),pbxy(x,y,t)); yt_xy.compute_root();
Func xt_yt("xt_yt"); xt_yt = dot(pbxt(x,y,t),pbyt(x,y,t)); xt_yt.compute_root();
Func xt_xy("xt_xy"); xt_xy = dot(pbxt(x,y,t),pbxy(x,y,t)); xt_xy.compute_root();
Func yt_yt("yt_yt"); yt_yt = dot(pbyt(x,y,t),pbyt(x,y,t)); yt_yt.compute_root();
Func xt_xt("xt_xt"); xt_xt = dot(pbxt(x,y,t),pbxt(x,y,t)); xt_xt.compute_root();
Func xy_xy("xy_xy"); xy_xy = dot(pbxy(x,y,t),pbxy(x,y,t)); xy_xy.compute_root();
Tuple Tk_tuple = Tuple(Tk[0](x,y,t),Tk[1](x,y,t),Tk[2](x,y,t),
Tk[3](x,y,t),Tk[4](x,y,t));
Func Tkd("Tkd"); Tkd = dot(Tk_tuple,Tk_tuple); Tkd.compute_root();
// Expr Dimen = pbxyd/xy_xy;
Expr kill(1.0f);
Func Oxy; Oxy(x,y,t) = Mdefdiv(st_filtered[5](x,y,t) - Mdefdivang(yt_xy(x,y,t),yt_yt(x,y,t),pbxyd(x,y,t),divisionthreshold2)*st_filtered[3](x,y,t)*kill,st_filtered[4](x,y,t),divisionthreshold);
Oxy.compute_root();
Func Oyx; Oyx(x,y,t) = Mdefdiv(st_filtered[5](x,y,t) + Mdefdivang(xt_xy(x,y,t),xt_xt(x,y,t),pbxyd(x,y,t),divisionthreshold2)*st_filtered[0](x,y,t)*kill,st_filtered[2](x,y,t),divisionthreshold);
Oyx.compute_root();
Func C0; C0(x,y,t) = st_filtered[3](x,y,t) * Mdefdivang(Expr(-1.0f)*xt_yt(x,y,t),yt_yt(x,y,t),pbxyd(x,y,t),divisionthreshold2)*kill;
C0.compute_root();
Func M0; M0(x,y,t) = Mdefdiv(st_filtered[0](x,y,t) + C0(x,y,t), st_filtered[1](x,y,t)*pow(Mdefdivang(xt_yt(x,y,t),yt_yt(x,y,t),pbxyd(x,y,t),divisionthreshold2),Expr(2.0f)),divisionthreshold);
M0.compute_root();
Func C1; C1(x,y,t) = st_filtered[5](x,y,t) * Mdefdivang(Expr(-1.0f)*xt_xy(x,y,t),xy_xy(x,y,t),pbxyd(x,y,t),divisionthreshold2)*kill;
C1.compute_root();
Func P1; P1(x,y,t) = pow(Mdefdivang(xt_yt(x,y,t),xt_xt(x,y,t),pbxyd(x,y,t),divisionthreshold2),Expr(2.0f))*kill + 1.0f;
P1.compute_root();
// 4 debug
// Func tmpOut("tmpOut"); tmpOut(x,y,t) = Tuple(Oxy(x,y,t),Oyx(x,y,t),C0(x,y,t),M0(x,y,t),C1(x,y,t),P1(x,y,t));
// return tmpOut;
Func Q1; Q1(x,y,t) = st_filtered[2](x,y,t) * (pow(Oyx(x,y,t),Expr(2.0f))+Expr(1.0f));
Q1.compute_root();
Func M1; M1(x,y,t) = Mdefdiv(((st_filtered[0](x,y,t)-C1(x,y,t))*P1(x,y,t)),Q1(x,y,t),divisionthreshold);
M1.compute_root();
Func C2; C2(x,y,t) = st_filtered[0](x,y,t) * Mdefdivang(Expr(-1.0f)*xt_yt(x,y,t),xt_xt(x,y,t),pbxyd(x,y,t),divisionthreshold2)*kill;
C2.compute_root();
Func M2; M2(x,y,t) = Mdefdiv(st_filtered[3](x,y,t)+C2(x,y,t),st_filtered[1](x,y,t)*(pow(Mdefdivang(xt_yt(x,y,t),xt_xt(x,y,t),pbxyd(x,y,t),divisionthreshold2),Expr(2.0f))*kill+Expr(1.0f)),divisionthreshold);
M2.compute_root();
Func C3; C3(x,y,t) = st_filtered[5](x,y,t) * Mdefdivang(yt_xy(x,y,t),xy_xy(x,y,t),pbxyd(x,y,t),divisionthreshold2)*kill;
C3.compute_root();
Func P3; P3(x,y,t) = pow(Mdefdivang(xt_yt(x,y,t),yt_yt(x,y,t),pbxyd(x,y,t),divisionthreshold2),Expr(2.0f))*kill + Expr(1.0f);
P3.compute_root();
Func Q3; Q3(x,y,t) = st_filtered[4](x,y,t) * (pow(Oxy(x,y,t),Expr(2.0f))+Expr(1.0f));
Q3.compute_root();
Func M3; M3(x,y,t) = Mdefdiv(((st_filtered[3](x,y,t)-C3(x,y,t))*P3(x,y,t)),Q3(x,y,t),divisionthreshold);
M3.compute_root();
Func basisAtAngle;
basisAtAngle(x,y,t) = Tuple(M0(x,y,t),M1(x,y,t),M2(x,y,t),M3(x,y,t),Tkd(x,y,t));
return basisAtAngle;
// Func hsv2rgb(Func colorImage) { // Took this function
// Var x, y, c, t;
// Func output;
// output(x,y,c,t) = cast <float> (0.0f);
// Expr fR, fG, fB; // R,G & B values
// Expr fH = (colorImage(x,y,0,t)); //H value [0-360)
// Expr fS = (colorImage(x,y,1,t)); //S value
// Expr fV = (colorImage(x,y,2,t)); //V value
// //Conversion (I took the one on Wikipedia)
// // https://fr.wikipedia.org/wiki/Teinte_Saturation_Valeur#Conversion_de_TSV_vers_RVB
// Expr fHi = floor(fH / Expr(60.0f));
// Expr fF = fH / 60.0f - fHi;
// Expr fL = fV * (1 - fS);
// Expr fM = fV * (1 - fF * fS) ;
// Expr fN = fV * (1 - (1 - fF) * fS);
// fR = select((0 == fHi),fV,
// (1 == fHi),fM,
// (2 == fHi),fL,
// (3 == fHi),fL,
// (4 == fHi),fN,
// (5 == fHi),fV,
// 0.0f);
// fG = select((0 == fHi),fN,
// (1 == fHi),fV,
// (2 == fHi),fV,
// (3 == fHi),fM,
// (4 == fHi),fL,
// (5 == fHi),fL,
// 0.0f);
// fB = select((0 == fHi),fL,
// (1 == fHi),fL,
// (2 == fHi),fN,
// (3 == fHi),fV,
// (4 == fHi),fV,
// (5 == fHi),fM,
// 0.0f);
// output(x,y,0,t) = fR;
// output(x,y,1,t) = fG;
// output(x,y,2,t) = fB;
// return output;
// }
// Func angle2rgb (Func v) {
// Var x, y, c, t;
// Func ov, a;
// ov(x,y,c,t) = cast <float> (0.0f);
// Expr pi2(2*M_PI);
// a(x,y,c,t) = v(x,y,c,t) / pi2;
// ov(x,y,0,t) = a(x,y,c,t);
// ov(x,y,1,t) = 1;
// ov(x,y,2,t) = 1;
// return ov;
// }
// Func outputvelocity(Func Blur, Func Speed, Func Angle, int border, Expr speedthreshold, Expr filterthreshold) {
// extern Expr width;
// extern Expr height;
// Func Blur3, Speed3;
// Blur3(x,y,c,t) = cast <float> (0.0f);
// Speed3(x,y,c,t) = cast <float> (0.0f);
// //Scale the grey level images
// Blur(x,y,0,t) = (Blur(x,y,0,t) - minimum(Blur(x,y,0,t))) / (maximum(Blur(x,y,0,t)) - minimum(Blur(x,y,0,t)));
// //Concatenation along the third dimension
// Blur3(x,y,0,t) = Blur(x,y,0,t);
// Blur3(x,y,1,t) = Blur(x,y,0,t);
// Blur3(x,y,2,t) = Blur(x,y,0,t);
// //Speed scaled to 1
// //Concatenation along the third dimension
// Speed3(x,y,1,t) = Speed(x,y,0,t);
// Speed3(x,y,2,t) = Speed(x,y,0,t);
// //Use the log speed to visualise speed
// Func LogSpeed;
// LogSpeed(x,y,c,t) = fast_log(Speed3(x,y,c,t) + Expr(0.0000001f))/fast_log(Expr(10.0f));
// LogSpeed(x,y,c,t) = (LogSpeed(x,y,c,t) - minimum(LogSpeed(x,y,c,t))) / (maximum(LogSpeed(x,y,c,t)) - minimum(LogSpeed(x,y,c,t)));
// //Make a colour image
// // uint16_t rows = height;
// // uint16_t cols = width;
// // int depth = Angle.channels();
// //Do it the HSV way
// Func colorImage;
// colorImage(x,y,0,t) = Angle(x,y,0,t);
// //Do hsv to rgb
// Func colorImage1;
// colorImage1 = hsv2rgb(colorImage);
// // Assume the border equals to the size of spatial filter
// //Make the border
// // int bir = rows + 2 * border;
// // int bic = cols + 2 * border;
// Expr orows = height / Expr(2);
// Expr ocols = width / Expr(2);
// //Rotation matrix
// int ph = 0;
// Func mb, sb;
// // if (rx < border - 1 || rx >= rows+border -1 || ry < border - 1 || ry >= cols+border - 1) {
// Expr co1 = x - orows;
// Expr co2 = - (y - ocols);
// Expr cosPh(cos(ph));
// Expr sinPh(sin(ph));
// Expr rco1 = cosPh * co1 - sinPh * co2; //Using rotation matrix
// Expr rco2 = sinPh * co1 + cosPh * co2;
// // Expr justPi (M_PI);
// mb(x,y,c,t) =
// select (((x < (border - 1)) ||
// (x >= (height+border -1)) ||
// (y < (border - 1)) ||
// (y >= (width+border - 1))),
// atan2(rco1,rco2) + Expr(M_PI),mb(x,y,c,t));
// sb(x,y,c,t) =
// select (((x < (border - 1)) ||
// (x >= (height+border -1)) ||
// (y < (border - 1) ) ||
// (y >= (width+border - 1))),
// 1, sb (x,y,c,t));
// Func cb;
// cb = angle2rgb(mb);
// //Get the old data
// // Expr pi2(2*M_PI);
// colorImage1(x,y,0,t)=colorImage(x,y,0,t) * Expr(2*M_PI);
// colorImage1=angle2rgb(colorImage1);
// colorImage1(x,y,c,t)=select(abs(Speed3(x,y,c,t))<speedthreshold,Expr(0.0f),colorImage1(x,y,c,t));
// Func colorImage2;
// colorImage2(x,y,c,t) = colorImage1(x,y,c,t) * Speed(x,y,c,t);
// //Put the data in the border
// RDom bordx (border,rows + border);
// RDom bordy (border,cols + border);
// Func ang1, ang2;
// ang1 (x,y,c,t) = cast <float> (0.0f);
// ang2 (x,y,c,t) = cast <float> (0.0f);
// cb(bordx, bordy,c,t) = colorImage1(x,y,c,t);
// ang1 = cb;
// cb(bordx, bordy,c,t) = colorImage2(x,y,c,t);
// ang2 = cb;
// sb(bordx, bordy,c,t) = Speed3(x,y,c,t);
// Speed3 = sb;
// sb(bordx, bordy,c,t) = Blur3(x,y,c,t);
// Blur3 = sb;
// // Func I;
// // I (x,y,c,t) = Blur3(x,y,c,t) + Speed3(x,y - height,c,t) + ang1(x - width,y,c,t) + ang2(x - width,y - height,c,t);
// //I = cat(2,cat(1,Blur,Speed),cat(1,ang1,ang2));
// return I;
// }
}
void slotChangedObject(const App::DocumentObject& Obj, const App::Property& Prop)
{
if (object == &Obj && Prop.getTypeId() == Part::PropertyGeometryList::getClassTypeId()) {
const Part::PropertyGeometryList& geom = static_cast<const Part::PropertyGeometryList&>(Prop);
const std::vector<Part::Geometry*>& items = geom.getValues();
if (items.size() != 2)
return;
Part::Geometry* g1 = items[0];
Part::Geometry* g2 = items[1];
if (!g1 || g1->getTypeId() != Part::GeomArcOfCircle::getClassTypeId())
return;
if (!g2 || g2->getTypeId() != Part::GeomLineSegment::getClassTypeId())
return;
Part::GeomArcOfCircle* arc = static_cast<Part::GeomArcOfCircle*>(g1);
Part::GeomLineSegment* seg = static_cast<Part::GeomLineSegment*>(g2);
try {
Base::Vector3d m1 = arc->getCenter();
//Base::Vector3d a3 = arc->getStartPoint();
Base::Vector3d a3 = arc->getEndPoint(true);
//Base::Vector3d l1 = seg->getStartPoint();
Base::Vector3d l2 = seg->getEndPoint();
#if 0
Py::Module pd("FilletArc");
Py::Callable method(pd.getAttr(std::string("makeFilletArc")));
Py::Callable vector(pd.getAttr(std::string("Vector")));
Py::Tuple xyz(3);
Py::Tuple args(6);
xyz.setItem(0, Py::Float(m1.x));
xyz.setItem(1, Py::Float(m1.y));
xyz.setItem(2, Py::Float(m1.z));
args.setItem(0,vector.apply(xyz));
xyz.setItem(0, Py::Float(a3.x));
xyz.setItem(1, Py::Float(a3.y));
xyz.setItem(2, Py::Float(a3.z));
args.setItem(1,vector.apply(xyz));
xyz.setItem(0, Py::Float(l2.x));
xyz.setItem(1, Py::Float(l2.y));
xyz.setItem(2, Py::Float(l2.z));
args.setItem(2,vector.apply(xyz));
xyz.setItem(0, Py::Float((float)0));
xyz.setItem(1, Py::Float((float)0));
xyz.setItem(2, Py::Float((float)1));
args.setItem(3,vector.apply(xyz));
args.setItem(4,Py::Float(radius));
args.setItem(5,Py::Int((int)0));
Py::Tuple ret(method.apply(args));
Py::Object S1(ret.getItem(0));
Py::Object S2(ret.getItem(1));
Py::Object M2(ret.getItem(2));
Base::Vector3d s1, s2, m2;
s1.x = (double)Py::Float(S1.getAttr("x"));
s1.y = (double)Py::Float(S1.getAttr("y"));
s1.z = (double)Py::Float(S1.getAttr("z"));
s2.x = (double)Py::Float(S2.getAttr("x"));
s2.y = (double)Py::Float(S2.getAttr("y"));
s2.z = (double)Py::Float(S2.getAttr("z"));
m2.x = (double)Py::Float(M2.getAttr("x"));
m2.y = (double)Py::Float(M2.getAttr("y"));
m2.z = (double)Py::Float(M2.getAttr("z"));
coords->point.set1Value(0, (float)s1.x,(float)s1.y,(float)s1.z);
coords->point.set1Value(1, (float)m2.x,(float)m2.y,(float)m2.z);
coords->point.set1Value(2, (float)s2.x,(float)s2.y,(float)s2.z);
Base::Console().Message("M1=<%.4f,%.4f>\n", m1.x,m1.y);
Base::Console().Message("M2=<%.4f,%.4f>\n", m2.x,m2.y);
Base::Console().Message("S1=<%.4f,%.4f>\n", s1.x,s1.y);
Base::Console().Message("S2=<%.4f,%.4f>\n", s2.x,s2.y);
Base::Console().Message("P=<%.4f,%.4f>\n", a3.x,a3.y);
Base::Console().Message("Q=<%.4f,%.4f>\n", l2.x,l2.y);
Base::Console().Message("\n");
#else
Py::Module pd("PartDesign");
Py::Callable method(pd.getAttr(std::string("makeFilletArc")));
Py::Tuple args(6);
args.setItem(0,Py::Vector(m1));
args.setItem(1,Py::Vector(a3));
args.setItem(2,Py::Vector(l2));
args.setItem(3,Py::Vector(Base::Vector3d(0,0,1)));
args.setItem(4,Py::Float(radius));
//args.setItem(5,Py::Int((int)0));
args.setItem(5,Py::Int((int)1));
Py::Tuple ret(method.apply(args));
Py::Vector S1(ret.getItem(0));
Py::Vector S2(ret.getItem(1));
Py::Vector M2(ret.getItem(2));
Base::Vector3d s1 = S1.toVector();
Base::Vector3d s2 = S2.toVector();
Base::Vector3d m2 = M2.toVector();
coords->point.set1Value(0, (float)s1.x,(float)s1.y,(float)s1.z);
coords->point.set1Value(1, (float)m2.x,(float)m2.y,(float)m2.z);
coords->point.set1Value(2, (float)s2.x,(float)s2.y,(float)s2.z);
Base::Console().Message("M1=<%.4f,%.4f>\n", m1.x,m1.y);
Base::Console().Message("M2=<%.4f,%.4f>\n", m2.x,m2.y);
Base::Console().Message("S1=<%.4f,%.4f>\n", s1.x,s1.y);
Base::Console().Message("S2=<%.4f,%.4f>\n", s2.x,s2.y);
Base::Console().Message("P=<%.4f,%.4f>\n", a3.x,a3.y);
Base::Console().Message("Q=<%.4f,%.4f>\n", l2.x,l2.y);
Base::Console().Message("\n");
#endif
}
catch (Py::Exception&) {
Base::PyException e; // extract the Python error text
Base::Console().Error("%s\n", e.what());
}
}
}
///////////////////////////////////////////////////////////////////////////////
// Creates a 4x4 rotation matrix, takes radians NOT degrees
void m3dRotationMatrix44(M3DMatrix44d m, double angle, double x, double y, double z)
{
double mag, s, c;
double xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
s = sin(angle);
c = cos(angle);
mag = sqrt( x*x + y*y + z*z );
// Identity matrix
if (mag == 0.0) {
m3dLoadIdentity44(m);
return;
}
// Rotation matrix is normalized
x /= mag;
y /= mag;
z /= mag;
#define M2(row,col) m[col*4+row]
xx = x * x;
yy = y * y;
zz = z * z;
xy = x * y;
yz = y * z;
zx = z * x;
xs = x * s;
ys = y * s;
zs = z * s;
one_c = 1.0f - c;
M2(0,0) = (one_c * xx) + c;
M2(0,1) = (one_c * xy) - zs;
M2(0,2) = (one_c * zx) + ys;
M2(0,3) = 0.0;
M2(1,0) = (one_c * xy) + zs;
M2(1,1) = (one_c * yy) + c;
M2(1,2) = (one_c * yz) - xs;
M2(1,3) = 0.0;
M2(2,0) = (one_c * zx) - ys;
M2(2,1) = (one_c * yz) + xs;
M2(2,2) = (one_c * zz) + c;
M2(2,3) = 0.0;
M2(3,0) = 0.0;
M2(3,1) = 0.0;
M2(3,2) = 0.0;
M2(3,3) = 1.0;
#undef M2
}