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