void MyWidget::Interpolation(uchar *bits, double x, double y, int *R, int *G, int *B) { int x0, y0, x1, y1; double dx, dy; if (x < _PIC_X - 1 && x >= 0 && y >= 0 && y < _PIC_Y - 1) { x0 = x; y0 = y; x1 = x0 + 1; y1 = y0 + 1; dx = x - x0; dy = y - y0; *R = Inter(Inter(bits[_ac[y0][x0] + 2], bits[_ac[y0][x1] + 2], dx), Inter(bits[_ac[y1][x0] + 2], bits[_ac[y1][x1] + 2], dx), dy); *G = Inter(Inter(bits[_ac[y0][x0] + 1], bits[_ac[y0][x1] + 1], dx), Inter(bits[_ac[y1][x0] + 1], bits[_ac[y1][x1] + 1], dx), dy); *B = Inter(Inter(bits[_ac[y0][x0]], bits[_ac[y0][x1]], dx), Inter(bits[_ac[y1][x0]], bits[_ac[y1][x1]], dx), dy); } else { *R = 0; *G = 0; *B = 0; } }
void MyWidget::Interpolation(double x, double y, int *R, int *G, int *B) { int x0, y0, x1, y1; double dx, dy; int a = x / 400; if (x > a * 400) { x = x - a * 400; } else { x = a * 400 - x; } a = y / 300; if (y > a * 300) { y = y - a * 300; } else { y = a * 300 - y; } x0 = x; y0 = y; x1 = x0 + 1; y1 = y0 + 1; dx = x - x0; dy = y - y0; *R = Inter(Inter(_bitsSrc[AC[y0][x0] + 2], _bitsSrc[AC[y0][x1] + 2], dx), Inter(_bitsSrc[AC[y1][x0] + 2], _bitsSrc[AC[y1][x1] + 2], dx), dy); *G = Inter(Inter(_bitsSrc[AC[y0][x0] + 1], _bitsSrc[AC[y0][x1] + 1], dx), Inter(_bitsSrc[AC[y1][x0] + 1], _bitsSrc[AC[y1][x1] + 1], dx), dy); *B = Inter(Inter(_bitsSrc[AC[y0][x0]], _bitsSrc[AC[y0][x1]], dx), Inter(_bitsSrc[AC[y1][x0]], _bitsSrc[AC[y1][x1]], dx), dy); }
//---------------------------------------------------------------------- void observer::Decremente(interval& X1,interval& Y1, interval& Theta1,interval& V1, interval& X,interval& Y,interval& Theta,interval& V,double u1,double u2) { if (phase==0) { X=Inter(X1,Xprior0[1]); Y=Inter(Y1,Xprior0[2]); Theta = Inter(Theta1,Xprior0[3]); V=Inter(V1,Xprior0[4]); } if (phase==1) { Theta = Theta1-dt*coef_ecart_moteur*(u2-u1); V = (V1-dt*coef_puissance_moteur*(u1+u2))/(1-dt*coef_friction); X = X1-dt*Cos(Theta)*V; Y = Y1-dt*Sin(Theta)*V; } }
//--------------------------------------------------------------------------- void observer::Incremente(interval& X1,interval& Y1, interval& Theta1,interval& V1, interval& X,interval& Y,interval& Theta,interval& V, double u1,double u2) { if (phase==0) { X1=Inter(X,Xprior0[1]); // La dynamique en phase d'initialisation est immobile Y1=Inter(Y,Xprior0[2]); // elle est différente de celle en mission Theta1=Inter(Theta,Xprior0[3]) ; V1=Inter(V,Xprior0[4]); } if (phase==1) { X1 = X + dt*Cos(Theta)*V; Y1 = Y + dt*Sin(Theta)*V; Theta1= Theta + dt*coef_ecart_moteur*(u2-u1); V1 = V + dt*(coef_puissance_moteur*(u1+u2)-coef_friction*V); } }
iboolean geq(iboolean x, iboolean y) // Project the constraint x>=y with respect to x { iboolean r; r=iboolean(iperhaps); if (y.value==empty) r=iboolean(empty); if (y.value==itrue) r=iboolean(itrue); return Inter(x,r); }
// Project the constraint x>=y with respect to x iboolean geq(iboolean x, iboolean y) { iboolean r; r = iboolean(iperhaps); if (y.value == iempty) r = iboolean(iempty); if (y.value == itrue) r = iboolean(itrue); return Inter(x,r); }
//---------------------------------------------------------------------- bool observer::Contract(box& P) { vector<box> L; vector<box> Pi(N_window); Pi[N_window-1]=P; // Dans ce qui suit, on recopie les listes dans des vecteurs pour avoir un accès direct (c'est pas terrible, mais je ne sais pas faire autrement) // Je sais qu'il existe une classe qui permet de faire une liste-vecteur, mais je ne sais pas son nom. vector<double> Vu1(N_window); list<double>::iterator iLu1=Lu1.begin(); for(int i=0;i<N_window;i++) {Vu1[i]=*iLu1; iLu1++;}; vector<double> Vu2(N_window); list<double>::iterator iLu2=Lu2.begin(); for(int i=0;i<N_window;i++) {Vu2[i]=*iLu2; iLu2++;}; vector<double> Valpha(N_window); list<double>::iterator iLalpha=Lalpha.begin(); for(int i=0;i<N_window;i++) {Valpha[i]=*iLalpha; iLalpha++;} vector<interval> Vd(N_window); list <interval>::iterator iLd=Ld.begin(); for(int i=0;i<N_window;i++) {Vd[i]=*iLd; iLd++;} vector<box> VXhat(N_window-1); list <box>::iterator iLXhat=LXhat.begin(); for(int i=0;i<N_window-1;i++){VXhat[i]=*iLXhat; iLXhat++;} for (int i=N_window-2;i>=0;i--) { Pi[i]=box(4); Decremente(Pi[i+1][1],Pi[i+1][2],Pi[i+1][3],Pi[i+1][4],Pi[i][1],Pi[i][2],Pi[i][3],Pi[i][4],Vu1[i],Vu2[i]); Pi[i]=Inter(Pi[i],VXhat[i]); } for (int i=0;i<N_window;i++) { interval xi=Pi[i][1]; interval yi=Pi[i][2]; interval thetai=Pi[i][3]; interval vi=Pi[i][4]; interval betai=Valpha[i]+thetai; interval di=Vd[i]; CLegOnWallsOrCircles(di,xi,yi,betai,murs_xa,murs_ya,murs_xb,murs_yb,cercles_x,cercles_y,cercles_r); Cplus(betai,Valpha[i],thetai,-1); interval xip,yip,thetaip,vip; for (int i1=i;i1<N_window-1;i1++) { Incremente(xip,yip,thetaip,vip,xi,yi,thetai,vi,Vu1[i1],Vu2[i1]); xip=Inter(xip,Pi[i1+1][1]);yip=Inter(yip,Pi[i1+1][2]); thetaip=Inter(thetaip,Pi[i1+1][3]);vip=Inter(vip,Pi[i1+1][4]); xi=xip; yi=yip; thetai=thetaip; vi=vip; } box Pb(P); Pb[1]=xi; Pb[2]=yi; Pb[3]=thetai; Pb[4]=vi; L.push_back(Pb); } vector<box> Lv=L; C_q_in(P,N_window-N_outliers,Lv); return true; }
unsigned long Synta::hexToLong(char* hex){ // char *boo; //waste point for strtol // char str[7]; //Destination of rearranged hex // strncpy(str,&hex[4],2); //Lower Byte // strncpy(str+2,&hex[2],2); //Middle Byte // strncpy(str+4,hex,2); //Upper Byte // str[6] = 0; // return strtol(str,&boo,16); //convert hex to long integer Inter inter = Inter(0,hexToByte(hex+4),hexToByte(hex+2),hexToByte(hex)); //create an inter return inter.integer; //and convert it to an integer }
void Synta::assembleResponse(char* dataPacket, char commandOrError, unsigned long responseData){ char replyLength = cmd.getLength(commandOrError,0); //get the number of data bytes for response //char tempStr[11]; if (replyLength == -1) { replyLength++; dataPacket[0] = errorChar; } else { dataPacket[0] = startOutChar; if (replyLength == 2) { Nibbler nibble = {responseData}; private_byteToHex(dataPacket+2,dataPacket+1,nibble); } else if (replyLength == 3) { DoubleNibbler nibble = {responseData}; nibbleToHex(dataPacket+3, nibble.low); nibbleToHex(dataPacket+2, nibble.mid); nibbleToHex(dataPacket+1, nibble.high); } else if (replyLength == 6) { Inter inter = Inter(responseData); private_byteToHex(dataPacket+6,dataPacket+5,inter.highByter.lowNibbler); private_byteToHex(dataPacket+4,dataPacket+3,inter.lowByter.highNibbler); private_byteToHex(dataPacket+2,dataPacket+1,inter.lowByter.lowNibbler); } } /* dataPacket++; switch (replyLength){ case -1: //error(dataPacket); //In otherwords, invalid command, so send error //return; break; case 0: //clearBuffer(dataPacket+1,sizeof(tempStr)); //empty temporary string break; case 2: byteToHex(dataPacket,responseData); break; case 3: intToHex(dataPacket,responseData); break; case 6: longToHex(dataPacket,responseData); break; }*/ dataPacket[(byte)replyLength + 1] = endChar; dataPacket[(byte)replyLength + 2] = 0; //success(dataPacket,tempStr,replyLength + 1); //compile response return; }
iboolean leq(iboolean x, iboolean y) // Project the constraint x<=y with respect to x { iboolean r; r=iboolean(iperhaps); if (y.value==empty) r=iboolean(empty); if (y.value==ifalse) r=iboolean(ifalse); return Inter(x,r); /* a && (implique b) 1*0=\empty 1 && 0 1*1=1 1 && [0,1] 1*[0,1]=1 1 && [0,1] 0*0=0 0 && 0 0*1=0 0 && [0,1] 0*[0,1]=0 0 && [0,1] [0,1]*0=0 [0,1] && 0 [0,1]*1=[0,1] [0,1] && [0,1] [0,1]*[0,1]=[0,1] [0,1] && [0,1] */ }
int main () { A a (0x12131415); long was = a.m.m; a = 0x22232425; if (was != now) printf("return 1\n");// we copied the empty base which clobbered a.m.m's // original value. A b (0x32333435); *(Inter *)&a = *(Inter *)&b; if (a.m.m != 0x22232425) printf("return 2\n"); // we copied padding, which clobbered a.m.m A b2 (0x32333435); (Inter &)b2 = Inter (); if (b2.m.m != 0x32333435) printf("return 2\n"); // we copied padding, which clobbered b2.m.m B c (0x12131415); was = c.m.m; c = 0x22232425; if (was != now) printf("return 3;\n"); B d (0x32333435); c.empty = d.empty; if (c.m.m != 0x22232425) printf("return 4;\n"); C e (0x32333435); if (e.m != 0x32333435) printf(" return 2\n"); // we copied padding printf("test ends\n"); }
// Project the constraint x<=y with respect to x iboolean leq(iboolean x, iboolean y) { iboolean r; r = iboolean(iperhaps); if (y.value == iempty) r = iboolean(iempty); if (y.value == ifalse) r = iboolean(ifalse); return Inter(x,r); /* a && (implique b) 1*0=\iempty 1 && 0 1*1=1 1 && [0,1] 1*[0,1]=1 1 && [0,1] 0*0=0 0 && 0 0*1=0 0 && [0,1] 0*[0,1]=0 0 && [0,1] [0,1]*0=0 [0,1] && 0 [0,1]*1=[0,1] [0,1] && [0,1] [0,1]*[0,1]=[0,1] [0,1] && [0,1] */ }
//--------------------------------------------------------------------------- bool observer::clock(double u1,double u2,interval d,double alpha, interval boussole) { // Au cas ou une boussole est accessible, il faut la ligne suivante // Sinon, la ligne suivante est inutile. Donc, dans tous les cas, il vaut mieux la laisser. interval sinTheta=Inter(Sin(Xhat[3]),Sin(boussole)); Csin(sinTheta,Xhat[3],-1); interval cosTheta=Inter(Cos(Xhat[3]),Cos(boussole)); Ccos(cosTheta,Xhat[3],-1); Lu1.push_back(u1); Lu2.push_back(u2); Ld.push_back(d); Lalpha.push_back(alpha); if ((int)Lu1.size()>N_window) { Lu1.pop_front(); Lu2.pop_front(); Ld.pop_front(); Lalpha.pop_front(); } if ((int)Lu1.size()==N_window) Xhat=SIVIA(Xhat,Xsecours); else { if (phase==0) Xhat=Xprior0; if (phase==1) return(false); // Cette ligne de code ne devrait jamais être atteinte. } LXhat.push_back(Xhat); if ((int)LXhat.size()>N_window-1) {LXhat.pop_front();} if (Xhat.IsEmpty()) { // An inconsistency is detected. The observer is not reliable anymore. //Form1->R1.DrawBox(Xpriork,clAqua,clAqua,bsSolid,1,2); return false; } //DrawChenille(Center(Xhat[1]),Center(Xhat[2]),Center(Xhat[3]),alpha); //DrawDiagram(); box Xhatp(4); Incremente(Xhatp[1],Xhatp[2],Xhatp[3],Xhatp[4],Xhat[1],Xhat[2],Xhat[3],Xhat[4],u1,u2); box StateNoise(4); interval Noise=interval(-0.1,0.1); StateNoise[1]=dt*Noise; StateNoise[2]=dt*Noise; StateNoise[3]=dt*Noise; StateNoise[4]=dt*Noise; Xhatp=Xhatp+StateNoise; Xhat=Xhatp; if ((Xhat.Width()>5)&(phase==1)) return false; //On ne se localise pas avec une assez bonne précision. On sort. return true; }
void BooleanGenerator::AddToTriangleBuffer(TriangleBuffer& Buffer) const { const VertexContainer& Vertices1 = this->FirstBuffer->GetVertices(); const IndexContainer& Indexes1 = this->FirstBuffer->GetIndices(); const VertexContainer& Vertices2 = this->SecondBuffer->GetVertices(); const IndexContainer& Indexes2 = this->SecondBuffer->GetIndices(); LineSegment3D IntersectionResult; IntersectContainer IntersectionList; // Find all intersections between FirstBuffer and SecondBuffer Integer FirstIndex = 0; for( IndexContainer::const_iterator FirstBufferIt = Indexes1.begin() ; FirstBufferIt != Indexes1.end() ; FirstIndex++ ) { Triangle3D Tri1( Vertices1[ *FirstBufferIt++ ].Position, Vertices1[ *FirstBufferIt++ ].Position, Vertices1[ *FirstBufferIt++ ].Position ); Integer SecondIndex = 0; for( IndexContainer::const_iterator SecondBufferIt = Indexes2.begin() ; SecondBufferIt != Indexes2.end() ; SecondIndex++ ) { Triangle3D Tri2( Vertices2[ *SecondBufferIt++ ].Position, Vertices2[ *SecondBufferIt++ ].Position, Vertices2[ *SecondBufferIt++ ].Position ); IntersectionResult = Tri1.GetOverlap(Tri2); if( IntersectionResult.PointA != IntersectionResult.PointB ) { Intersect Inter(IntersectionResult,FirstIndex,SecondIndex); IntersectionList.push_back(Inter); } } } // Remove all intersection segments too small to be relevant for( IntersectContainer::iterator InterIt = IntersectionList.begin() ; InterIt != IntersectionList.end() ; ) { if( ( InterIt->Segment.PointB - InterIt->Segment.PointA ).SquaredLength() < 1e-8 ) { InterIt = IntersectionList.erase(InterIt); }else{ ++InterIt; } } // Retriangulate TriangleBuffer NewMesh1, NewMesh2; _Retriangulate(NewMesh1,*(this->FirstBuffer),IntersectionList,true); _Retriangulate(NewMesh2,*(this->SecondBuffer),IntersectionList,false); //Buffer.append(NewMesh1); //Buffer.append(NewMesh2); //return; // Trace contours PathContainer Contours; LineSeg3DVec SegmentSoup; for( IntersectContainer::iterator InterIt = IntersectionList.begin() ; InterIt != IntersectionList.end() ; ++InterIt ) { SegmentSoup.push_back( InterIt->Segment ); } Path::BuildFromSegmentSoup(SegmentSoup,Contours); // Build a lookup from segment to triangle TriLookup TriLookup1, TriLookup2; _BuildTriLookup( TriLookup1, NewMesh1 ); _BuildTriLookup( TriLookup2, NewMesh2 ); LineSeg3DSet Limits; for( LineSeg3DVec::iterator SegSoupIt = SegmentSoup.begin() ; SegSoupIt != SegmentSoup.end() ; ++SegSoupIt ) { Limits.insert( SegSoupIt->GetOrderedCopy() ); } // Build resulting mesh for( PathContainer::iterator PathIt = Contours.begin() ; PathIt != Contours.end() ; ++PathIt ) { // Find 2 seed triangles for each contour LineSegment3D FirstSeg( PathIt->GetPoint(0), PathIt->GetPoint(1) ); TriLookupRange It2mesh1 = TriLookup1.equal_range( FirstSeg.GetOrderedCopy() ); TriLookupRange It2mesh2 = TriLookup2.equal_range( FirstSeg.GetOrderedCopy() ); Integer Mesh1Seed1, Mesh1Seed2, Mesh2Seed1, Mesh2Seed2; if( It2mesh1.first != TriLookup1.end() && It2mesh2.first != TriLookup2.end() ) { // check which of seed1 and seed2 must be included (it can be 0, 1 or both) Mesh1Seed1 = It2mesh1.first->second; Mesh1Seed2 = (--It2mesh1.second)->second; Mesh2Seed1 = It2mesh2.first->second; Mesh2Seed2 = (--It2mesh2.second)->second; if( Mesh1Seed1 == Mesh1Seed2 ) { Mesh1Seed2 = -1; } if( Mesh2Seed1 == Mesh2Seed2 ) { Mesh2Seed2 = -1; } Vector3 vMesh1, nMesh1, vMesh2, nMesh2; for( Integer i = 0 ; i < 3 ; i++ ) { const Vector3& Position = NewMesh1.GetVertices()[ NewMesh1.GetIndices()[ Mesh1Seed1 * 3 + i ] ].Position; if( Position.SquaredDistance( FirstSeg.PointA ) > 1e-6 && Position.SquaredDistance( FirstSeg.PointB ) > 1e-6 ) { vMesh1 = Position; nMesh1 = NewMesh1.GetVertices()[ NewMesh1.GetIndices()[ Mesh1Seed1 * 3 + i ] ].Normal; break; } } for( Integer i = 0 ; i < 3 ; i++ ) { const Vector3& Position = NewMesh2.GetVertices()[ NewMesh2.GetIndices()[ Mesh2Seed1 * 3 + i ] ].Position; if( Position.SquaredDistance( FirstSeg.PointA ) > 1e-6 && Position.SquaredDistance( FirstSeg.PointB ) > 1e-6 ) { vMesh2 = Position; nMesh2 = NewMesh2.GetVertices()[ NewMesh2.GetIndices()[ Mesh2Seed1 * 3 + i ] ].Normal; break; } } Boole M2S1InsideM1 = ( nMesh1.DotProduct( vMesh2 - FirstSeg.PointA ) < 0 ); Boole M1S1InsideM2 = ( nMesh2.DotProduct( vMesh1 - FirstSeg.PointA ) < 0 ); _RemoveFromTriLookup( Mesh1Seed1, TriLookup1 ); _RemoveFromTriLookup( Mesh2Seed1, TriLookup2 ); _RemoveFromTriLookup( Mesh1Seed2, TriLookup1 ); _RemoveFromTriLookup( Mesh2Seed2, TriLookup2 ); // Recursively add all neighbours of these triangles // Stop when a contour is touched switch( this->BoolOp ) { case BO_Union: { if( M1S1InsideM2 ) { _RecursiveAddNeighbour(Buffer,NewMesh1,Mesh1Seed2,TriLookup1,Limits,false); }else{ _RecursiveAddNeighbour(Buffer,NewMesh1,Mesh1Seed1,TriLookup1,Limits,false); } if( M2S1InsideM1 ) { _RecursiveAddNeighbour(Buffer,NewMesh2,Mesh2Seed2,TriLookup2,Limits,false); }else{ _RecursiveAddNeighbour(Buffer,NewMesh2,Mesh2Seed1,TriLookup2,Limits,false); } break; } case BO_Intersection: { if( M1S1InsideM2 ) { _RecursiveAddNeighbour(Buffer,NewMesh1,Mesh1Seed1,TriLookup1,Limits,false); }else{ _RecursiveAddNeighbour(Buffer,NewMesh1,Mesh1Seed2,TriLookup1,Limits,false); } if( M2S1InsideM1 ) { _RecursiveAddNeighbour(Buffer,NewMesh2,Mesh2Seed1,TriLookup2,Limits,false); }else{ _RecursiveAddNeighbour(Buffer,NewMesh2,Mesh2Seed2,TriLookup2,Limits,false); } break; } case BO_Difference: { if( M1S1InsideM2 ) { _RecursiveAddNeighbour(Buffer,NewMesh1,Mesh1Seed2,TriLookup1,Limits,false); }else{ _RecursiveAddNeighbour(Buffer,NewMesh1,Mesh1Seed1,TriLookup1,Limits,false); } if( M2S1InsideM1 ) { _RecursiveAddNeighbour(Buffer,NewMesh2,Mesh2Seed1,TriLookup2,Limits,true); }else{ _RecursiveAddNeighbour(Buffer,NewMesh2,Mesh2Seed2,TriLookup2,Limits,true); } break; } } } } }