void TestSymBandMatrixArith_B1()
{
std::vector<tmv::SymBandMatrixView<T> > sb;
std::vector<tmv::SymBandMatrixView<std::complex<T> > > csb;
MakeSymBandList(sb,csb,InDef);
const int N = sb[0].size();
tmv::Matrix<T> a1(N,N);
for (int i=0; i<N; ++i) for (int j=0; j<N; ++j) a1(i,j) = T(3+i-5*j);
tmv::Matrix<std::complex<T> > ca1(N,N);
for (int i=0; i<N; ++i) for (int j=0; j<N; ++j) ca1(i,j) =
std::complex<T>(3+i-5*j,2-3*i);
tmv::MatrixView<T> a1v = a1.view();
tmv::MatrixView<std::complex<T> > ca1v = ca1.view();
#if (XTEST & 2)
tmv::Matrix<T> a2(2*N,2*N);
for (int i=0; i<2*N; ++i) for (int j=0; j<2*N; ++j) a2(i,j) = T(1-3*i+6*j);
tmv::Matrix<std::complex<T> > ca2(2*N,2*N);
for (int i=0; i<2*N; ++i) for (int j=0; j<2*N; ++j) ca2(i,j) =
std::complex<T>(1-3*i+6*j,-4+2*j);
tmv::Matrix<T,tmv::RowMajor> a3 = a2.rowRange(0,N);
tmv::Matrix<std::complex<T> > ca3 = a3 * std::complex<T>(-3,4);
tmv::Matrix<T,tmv::RowMajor> a4 = a1.colRange(0,0);
tmv::Matrix<std::complex<T> > ca4 = a4;
tmv::MatrixView<T> a3v = a3.view();
tmv::MatrixView<T> a4v = a4.view();
tmv::MatrixView<std::complex<T> > ca3v = ca3.view();
tmv::MatrixView<std::complex<T> > ca4v = ca4.view();
#endif
for(size_t i=START;i<sb.size();i++) {
if (showstartdone) {
std::cout<<"Start loop i = "<<i<<std::endl;
std::cout<<"si = "<<sb[i]<<std::endl;
}
tmv::SymBandMatrixView<T> si = sb[i];
tmv::SymBandMatrixView<std::complex<T> > csi = csb[i];
TestMatrixArith4(si,csi,a1v,ca1v,"SymBand/SquareM");
TestMatrixArith5(si,csi,a1v,ca1v,"SymBand/SquareM");
TestMatrixArith6x(si,csi,a1v,ca1v,"SymBand/SquareM");
#if (XTEST & 2)
TestMatrixArith4(si,csi,a3v,ca3v,"SymBand/NonSquareM");
TestMatrixArith5(si,csi,a3v,ca3v,"SymBand/NonSquareM");
TestMatrixArith6x(si,csi,a3v,ca3v,"SymBand/NonSquareM");
TestMatrixArith4(si,csi,a4v,ca4v,"SymBand/DegenerateM");
TestMatrixArith5(si,csi,a4v,ca4v,"SymBand/DegenerateM");
TestMatrixArith6x(si,csi,a4v,ca4v,"SymBand/DegenerateM");
#endif
}
}
/* ****************************************************************************
*
* check_json -
*/
TEST(ContextAttributeResponseVector, check_json)
{
ContextAttributeResponseVector carV;
ContextAttribute ca("caName", "caType", "caValue");
ContextAttributeResponse car;
std::string out;
const char* outfile1 = "ngsi10.contextAttributeResponse.check1.valid.json";
const char* outfile2 = "ngsi10.contextAttributeResponse.check2.valid.json";
ConnectionInfo ci(JSON);
// 1. ok
car.contextAttributeVector.push_back(&ca);
carV.push_back(&car);
out = carV.check(&ci, UpdateContextAttribute, "", "", 0);
EXPECT_STREQ("OK", out.c_str());
// 2. Predetected Error
out = carV.check(&ci, UpdateContextAttribute, "", "PRE ERROR", 0);
EXPECT_EQ("OK", testDataFromFile(expectedBuf, sizeof(expectedBuf), outfile1)) << "Error getting test data from '" << outfile1 << "'";
EXPECT_STREQ(expectedBuf, out.c_str());
// 3. Bad ContextAttribute
ContextAttribute ca2("", "caType", "caValue");
car.contextAttributeVector.push_back(&ca2);
out = carV.check(&ci, UpdateContextAttribute, "", "", 0);
EXPECT_EQ("OK", testDataFromFile(expectedBuf, sizeof(expectedBuf), outfile2)) << "Error getting test data from '" << outfile2 << "'";
EXPECT_STREQ(expectedBuf, out.c_str());
}
template <class T> void TestTriDiv_A2()
{
const int N = 10;
tmv::Matrix<T> m(N,N);
for (int i=0; i<N; ++i) for (int j=0; j<N; ++j)
m(i,j) = T(0.4+0.02*i-0.05*j);
m.diag().addToAll(5);
m.diag(1).addToAll(T(0.32));
m.diag(-1).addToAll(T(0.91));
tmv::Matrix<std::complex<T> > cm(m);
cm += std::complex<T>(10,2);
cm.diag(1) *= std::complex<T>(T(-0.5),T(-0.8));
cm.diag(-1) *= std::complex<T>(T(-0.7),T(0.1));
tmv::UpperTriMatrix<T,tmv::NonUnitDiag> a1(m);
tmv::UpperTriMatrix<std::complex<T>,tmv::NonUnitDiag> ca1(cm);
tmv::UpperTriMatrix<T,tmv::UnitDiag> a2(m);
tmv::UpperTriMatrix<std::complex<T>,tmv::UnitDiag> ca2(cm);
tmv::UpperTriMatrixView<T> a1v = a1.view();
tmv::UpperTriMatrixView<T> a2v = a2.view();
tmv::UpperTriMatrixView<std::complex<T> > ca1v = ca1.view();
tmv::UpperTriMatrixView<std::complex<T> > ca2v = ca2.view();
tmv::LowerTriMatrixView<T> a1t = a1.transpose();
tmv::LowerTriMatrixView<T> a2t = a2.transpose();
tmv::LowerTriMatrixView<std::complex<T> > ca1t = ca1.transpose();
tmv::LowerTriMatrixView<std::complex<T> > ca2t = ca2.transpose();
TestMatrixDivArith1(tmv::LU,a1v,a2t,ca1v,ca2t,"L/U");
TestMatrixDivArith1(tmv::LU,a1t,a2v,ca1t,ca2v,"U/L");
TestMatrixDivArith1(tmv::LU,a2v,a1t,ca2v,ca1t,"L/U");
TestMatrixDivArith1(tmv::LU,a2t,a1v,ca2t,ca1v,"U/L");
#if (XTEST & 2)
tmv::UpperTriMatrix<T,tmv::NonUnitDiag> a1b(m);
tmv::UpperTriMatrix<std::complex<T>,tmv::NonUnitDiag> ca1b(cm);
tmv::UpperTriMatrix<T,tmv::UnitDiag> a2b(m);
tmv::UpperTriMatrix<std::complex<T>,tmv::UnitDiag> ca2b(cm);
tmv::UpperTriMatrixView<T> a1bv = a1b.view();
tmv::UpperTriMatrixView<T> a2bv = a2b.view();
tmv::UpperTriMatrixView<std::complex<T> > ca1bv = ca1b.view();
tmv::UpperTriMatrixView<std::complex<T> > ca2bv = ca2b.view();
tmv::LowerTriMatrixView<T> a1bt = a1b.transpose();
tmv::LowerTriMatrixView<T> a2bt = a2b.transpose();
tmv::LowerTriMatrixView<std::complex<T> > ca1bt = ca1b.transpose();
tmv::LowerTriMatrixView<std::complex<T> > ca2bt = ca2b.transpose();
TestMatrixDivArith1(tmv::LU,a1v,a1bt,ca1v,ca1bt,"L/U");
TestMatrixDivArith1(tmv::LU,a1t,a1bv,ca1t,ca1bv,"U/L");
TestMatrixDivArith1(tmv::LU,a2v,a2bt,ca2v,ca2bt,"L/U");
TestMatrixDivArith1(tmv::LU,a2t,a2bv,ca2t,ca2bv,"U/L");
#endif
}
/* ****************************************************************************
*
* check_json -
*/
TEST(AppendContextElementRequest, check_json)
{
AppendContextElementRequest acer;
std::string out;
ContextAttribute ca("caName", "caType", "121");
Metadata md("mdName", "mdType", "122");
const char* outfile1 = "ngsi10.appendContextElementResponse.predetectedError.valid.json";
const char* outfile2 = "ngsi10.appendContextElementResponse.missingAttributeName.valid.json";
const char* outfile3 = "ngsi10.appendContextElementResponse.missingMetadataName.valid.json";
ConnectionInfo ci;
utInit();
acer.attributeDomainName.set("ADN");
acer.contextAttributeVector.push_back(&ca);
acer.domainMetadataVector.push_back(&md);
// 1. ok
ci.outMimeType = JSON;
out = acer.check(&ci, AppendContextElement, "", "", 0);
EXPECT_STREQ("OK", out.c_str());
// 2. Predetected error
EXPECT_EQ("OK", testDataFromFile(expectedBuf, sizeof(expectedBuf), outfile1)) << "Error getting test data from '" << outfile1 << "'";
out = acer.check(&ci, AppendContextElement, "", "Error is predetected", 0);
EXPECT_STREQ(expectedBuf, out.c_str());
// 3. bad ContextAttribute
ContextAttribute ca2("", "caType", "121");
acer.contextAttributeVector.push_back(&ca2);
out = acer.check(&ci, AppendContextElement, "", "", 0);
EXPECT_EQ("OK", testDataFromFile(expectedBuf, sizeof(expectedBuf), outfile2)) << "Error getting test data from '" << outfile2 << "'";
EXPECT_STREQ(expectedBuf, out.c_str());
ca2.name = "ca2Name";
// 4. Bad domainMetadata
Metadata md2("", "mdType", "122");
acer.domainMetadataVector.push_back(&md2);
out = acer.check(&ci, AppendContextElement, "", "", 0);
EXPECT_EQ("OK", testDataFromFile(expectedBuf, sizeof(expectedBuf), outfile3)) << "Error getting test data from '" << outfile3 << "'";
EXPECT_STREQ(expectedBuf, out.c_str());
// 5. Bad attributeDomainName
// FIXME P3: AttributeDomainName::check always returns "OK"
utExit();
}
/* ****************************************************************************
*
* noPatternMultiAttr -
*
* Discover: E1 - (A3, A4, A5)
* Result: E1 - A3 - http://cr1.com
* E1 - A4 - http://cr2.com
*/
TEST(mongoContextProvidersUpdateRequest, noPatternMultiAttr)
{
HttpStatusCode ms;
UpdateContextRequest req;
UpdateContextResponse res;
/* Prepare database */
utInit();
prepareDatabase();
/* Forge the request (from "inside" to "outside") */
ContextElement ce;
ce.entityId.fill("E1", "T1", "false");
ContextAttribute ca1("A3", "TA3", "new_val");
ContextAttribute ca2("A4", "TA4", "new_val");
ContextAttribute ca3("A5", "TA5", "new_val");
ce.contextAttributeVector.push_back(&ca1);
ce.contextAttributeVector.push_back(&ca2);
ce.contextAttributeVector.push_back(&ca3);
req.contextElementVector.push_back(&ce);
req.updateActionType.set("UPDATE");
/* Invoke the function in mongoBackend library */
ms = mongoUpdateContext(&req, &res, "", servicePathVector, uriParams);
/* Check response is as expected */
EXPECT_EQ(SccOk, ms);
EXPECT_EQ(SccNone, res.errorCode.code);
EXPECT_EQ(0, res.errorCode.reasonPhrase.size());
EXPECT_EQ(0, res.errorCode.details.size());
ASSERT_EQ(1, res.contextElementResponseVector.size());
EXPECT_EQ("E1", RES_CER(0).entityId.id);
EXPECT_EQ("T1", RES_CER(0).entityId.type);
EXPECT_EQ("false", RES_CER(0).entityId.isPattern);
ASSERT_EQ(3, RES_CER(0).contextAttributeVector.size());
EXPECT_EQ("A3", RES_CER_ATTR(0, 0)->name);
EXPECT_EQ("TA3", RES_CER_ATTR(0, 0)->type);
EXPECT_EQ("A4", RES_CER_ATTR(0, 1)->name);
EXPECT_EQ("TA4", RES_CER_ATTR(0, 1)->type);
EXPECT_EQ("A5", RES_CER_ATTR(0, 2)->name);
EXPECT_EQ("TA5", RES_CER_ATTR(0, 2)->type);
EXPECT_EQ(SccFound, RES_CER_STATUS(0).code);
EXPECT_EQ("Found", RES_CER_STATUS(0).reasonPhrase);
EXPECT_EQ("http://cr1.com", RES_CER_STATUS(0).details);
/* Release connection */
mongoDisconnect();
utExit();
}
int main()
{
A a;
B b;
C c;
std::shared_ptr<A> ca1(a.clone());
std::shared_ptr<A> cb1(b.clone());
std::shared_ptr<A> cc1(c.clone());
ca1->tell();
cb1->tell();
cc1->tell();
std::shared_ptr<A> ca2(ca1->clone());
std::shared_ptr<A> cb2(cb1->clone());
std::shared_ptr<A> cc2(cc1->clone());
ca2->tell();
cb2->tell();
cc2->tell();
return 0;
}
/* ****************************************************************************
*
* patternNAttr -
*
* Discover: E[1-2] - (A1, A2)
* Result: (E1. E2) - (A1, A2) - http://cr1.com
* E1 - A1 - http://cr2.com
* E2 - A2 - http://cr3.com
*
* isPattern=true is not currently supported in updateContext, so this test it disabled: enable it once
* this gets supported (may need some extra modification to work)
*/
TEST(DISABLED_mongoContextProvidersUpdateRequest, patternNAttr)
{
HttpStatusCode ms;
UpdateContextRequest req;
UpdateContextResponse res;
/* Prepare database */
utInit();
prepareDatabasePatternTrue();
/* Forge the request (from "inside" to "outside") */
ContextElement ce;
ce.entityId.fill("E[1-2]", "T", "true");
ContextAttribute ca1("A1", "TA1", "new_val");
ContextAttribute ca2("A2", "TA2", "new_val");
ce.contextAttributeVector.push_back(&ca1);
ce.contextAttributeVector.push_back(&ca2);
req.contextElementVector.push_back(&ce);
req.updateActionType.set("UPDATE");
/* Invoke the function in mongoBackend library */
ms = mongoUpdateContext(&req, &res, "", servicePathVector, uriParams);
/* Check response is as expected */
EXPECT_EQ(SccOk, ms);
EXPECT_EQ(SccFound, res.errorCode.code);
EXPECT_EQ("Found", res.errorCode.reasonPhrase);
EXPECT_EQ("http://cr1.com", res.errorCode.details);
ASSERT_EQ(0, res.contextElementResponseVector.size());
/* Release connection */
mongoDisconnect();
utExit();
}
/* ****************************************************************************
*
* check_json -
*/
TEST(ContextAttributeResponse, check_json)
{
ContextAttribute ca("caName", "caType", "caValue");
ContextAttributeResponse car;
std::string out;
const char* outfile1 = "ngsi10.contextAttributeResponse.check3.valid.json";
const char* outfile2 = "ngsi10.contextAttributeResponse.check4.valid.json";
ConnectionInfo ci;
utInit();
// 1. OK
car.contextAttributeVector.push_back(&ca);
car.statusCode.fill(SccOk, "OK");
ci.outFormat = JSON;
out = car.check(&ci, UpdateContextAttribute, "", "", 0);
EXPECT_STREQ("OK", out.c_str());
// 2. predetectedError
out = car.check(&ci, UpdateContextAttribute, "", "PRE Error", 0);
EXPECT_EQ("OK", testDataFromFile(expectedBuf, sizeof(expectedBuf), outfile1)) << "Error getting test data from '" << outfile1 << "'";
EXPECT_STREQ(expectedBuf, out.c_str());
// 3. Bad ContextAttribute
ContextAttribute ca2("", "caType", "caValue");
car.contextAttributeVector.push_back(&ca2);
LM_M(("car.contextAttributeVector.size: %d - calling ContextAttributeResponse::check", car.contextAttributeVector.size()));
out = car.check(&ci, UpdateContextAttribute, "", "", 0);
EXPECT_EQ("OK", testDataFromFile(expectedBuf, sizeof(expectedBuf), outfile2)) << "Error getting test data from '" << outfile2 << "'";
EXPECT_STREQ(expectedBuf, out.c_str());
utExit();
}
Sivia::Sivia(repere& R, struct sivia_struct *par) : R(R) {
par->area = 0;
// Create the function we want to apply SIVIA on.
Variable x,y;
double ei = par->ei;
double xb=par->xb1,yb=par->yb1;
Interval xbi=Interval(par->xb1-ei,par->xb1+ei),ybi=Interval(par->yb1-ei,par->yb1+ei);
double arc = par->sonar_arc;
double r = pow(par->sonar_radius,2);
double th1 = par->th[0];
double th2=th1+arc;
double th21= par->th[1];
double th22=th21 + arc;
double th31= par->th[2];
double th32=th31 + arc;
double e=1;
double epsilon = par->epsilon;
double xin,yin;
// First SONAR
Function f(x,y,sqr(x-xbi)+sqr(y-ybi));
NumConstraint c1(x,y,f(x,y)<=r+e);
NumConstraint c2(x,y,f(x,y)>=e);
NumConstraint c3(x,y,f(x,y)>r+e);
NumConstraint c4(x,y,f(x,y)<e);
double sign1,sign2;
if(cos(th1)>0) sign1=1;
else sign1=-1;
if(cos(th2)<0) sign2=1;
else sign2=-1;
NumConstraint cth11(x,y,sign1*(y-ybi-((sin(th1))/(cos(th1)))*(x-xbi))<0);
NumConstraint cth12(x,y,sign1*(y-ybi-((sin(th1))/(cos(th1)))*(x-xbi))>0);
NumConstraint cth21(x,y,sign2*(y-ybi-((sin(th2))/(cos(th2)))*(x-xbi))<0);
NumConstraint cth22(x,y,sign2*(y-ybi-((sin(th2))/(cos(th2)))*(x-xbi))>0);
// Create contractors with respect to each
// of the previous constraints.
CtcFwdBwd out1(c1);
CtcFwdBwd out2(c2);
CtcFwdBwd in1(c3);
CtcFwdBwd in2(c4);
CtcFwdBwd outth1(cth12);
CtcFwdBwd inth1(cth11);
CtcFwdBwd inth2(cth21);
CtcFwdBwd outth2(cth22);
// CtcIn inside(f,Interval(-1,1));
// CtcNotIn outside(f,Interval(-1,1));
// Create a contractor that removes all the points
// that do not satisfy either f(x,y)<=2 or f(x,y)>=0.
// These points are "outside" of the solution set.
CtcCompo outside1(out1,out2,outth1,outth2);
// Create a contractor that removes all the points
// that do not satisfy both f(x,y)>2 or f(x,y)<0.
// These points are "inside" the solution set.
CtcUnion inside11(in1,in2,inth1);
CtcUnion inside1(inside11,inth2);
// Second SONAR
double xb2=par->xb2,yb2=par->yb2;
Interval xb2i=Interval(par->xb2-ei,par->xb2+ei),yb2i=Interval(par->yb2-ei,par->yb2+ei);
Function f2(x,y,sqr(x-xb2i)+sqr(y-yb2i));
NumConstraint c21(x,y,f2(x,y)<=r+e);
NumConstraint c22(x,y,f2(x,y)>=e);
NumConstraint c23(x,y,f2(x,y)>r+e);
NumConstraint c24(x,y,f2(x,y)<e);
double sign21,sign22;
if(cos(th21)>0) sign21=-1;
else sign21=1;
if(cos(th22)<0) sign22=1;
else sign22=-1;
NumConstraint cth211(x,y,sign21*(y-yb2i-((sin(th21))/(cos(th21)))*(x-xb2i))<0);
NumConstraint cth212(x,y,sign21*(y-yb2i-((sin(th21))/(cos(th21)))*(x-xb2i))>0);
NumConstraint cth221(x,y,sign22*(y-yb2i-((sin(th22))/(cos(th22)))*(x-xb2i))<0);
NumConstraint cth222(x,y,sign22*(y-yb2i-((sin(th22))/(cos(th22)))*(x-xb2i))>0);
// Create contractors with respect to each
// of the previous constraints.
CtcFwdBwd out21(c21);
CtcFwdBwd out22(c22);
CtcFwdBwd in21(c23);
CtcFwdBwd in22(c24);
CtcFwdBwd outth21(cth211);
CtcFwdBwd inth21(cth212);
CtcFwdBwd inth22(cth221);
CtcFwdBwd outth22(cth222);
// CtcIn inside(f,Interval(-1,1));
// CtcNotIn outside(f,Interval(-1,1));
// Create a contractor that removes all the points
// that do not satisfy either f(x,y)<=2 or f(x,y)>=0.
// These points are "outside" of the solution set.
CtcCompo outside2(out21,out22,outth21,outth22);
// Create a contractor that removes all the points
// that do not satisfy both f(x,y)>2 or f(x,y)<0.
// These points are "inside" the solution set.
CtcUnion inside21(in21,in22,inth21);
CtcUnion inside2(inside21,inth22);
//Third SONAR
double xb3=par->xb3,yb3=par->yb3;
Interval xb3i=Interval(par->xb3-ei,par->xb3+ei),yb3i=Interval(par->yb3-ei,par->yb3+ei);
Function f3(x,y,sqr(x-xb3i)+sqr(y-yb3i));
NumConstraint c31(x,y,f3(x,y)<=r+e);
NumConstraint c32(x,y,f3(x,y)>=e);
NumConstraint c33(x,y,f3(x,y)>r+e);
NumConstraint c34(x,y,f3(x,y)<e);
double sign31,sign32;
if(cos(th31)>0) sign31=-1;
else sign31=1;
if(cos(th32)<0) sign32=1;
else sign32=-1;
NumConstraint cth311(x,y,sign31*(y-yb3i-((sin(th31))/(cos(th31)))*(x-xb3i))<0);
NumConstraint cth312(x,y,sign31*(y-yb3i-((sin(th31))/(cos(th31)))*(x-xb3i))>0);
NumConstraint cth321(x,y,sign32*(y-yb3i-((sin(th32))/(cos(th32)))*(x-xb3i))<0);
NumConstraint cth322(x,y,sign32*(y-yb3i-((sin(th32))/(cos(th32)))*(x-xb3i))>0);
// Create contractors with respect to each
// of the previous constraints.
CtcFwdBwd out31(c31);
CtcFwdBwd out32(c32);
CtcFwdBwd in31(c33);
CtcFwdBwd in32(c34);
CtcFwdBwd outth31(cth311);
CtcFwdBwd inth31(cth312);
CtcFwdBwd inth32(cth321);
CtcFwdBwd outth32(cth322);
// CtcIn inside(f,Interval(-1,1));
// CtcNotIn outside(f,Interval(-1,1));
// Create a contractor that removes all the points
// that do not satisfy either f(x,y)<=2 or f(x,y)>=0.
// These points are "outside" of the solution set.
CtcCompo outside3(out31,out32,outth31,outth32);
// Create a contractor that removes all the points
// that do not satisfy both f(x,y)>2 or f(x,y)<0.
// These points are "inside" the solution set.
CtcUnion inside31(in31,in32,inth31);
CtcUnion inside3(inside31,inth32);
//CtcQInter inter(inside,1);
//Artifact MODELISATION
double xa = par->xa;
double ya = par->ya;
double ra = par->ra;
Function f_a(x,y,sqr(x-xa)+sqr(y-ya));
NumConstraint ca1(x,y,f_a(x,y)<=sqr(ra));
NumConstraint ca2(x,y,f_a(x,y)>=sqr(ra)-par->thick);
NumConstraint ca3(x,y,f_a(x,y)>sqr(ra));
NumConstraint ca4(x,y,f_a(x,y)<sqr(ra)-par->thick);
CtcFwdBwd aout1(ca1);
CtcFwdBwd aout2(ca2);
CtcFwdBwd ain1(ca3);
CtcFwdBwd ain2(ca4);
CtcUnion ain(ain1,ain2);
CtcCompo aout(aout1,aout2);
//Robot MODELISATION
double xr = par->xr; //robot position x
double yr = par->yr; //robot position y
double wr = par->wr; //robot width
double lr = par->lr; //robot length
double ep = par->thick;
xr = par->xr - wr/2;
NumConstraint inrx1(x,y,x>xr+ep);
NumConstraint outrx1(x,y,x<xr+ep);
NumConstraint inrx2(x,y,x<xr-ep);
NumConstraint outrx2(x,y,x>xr-ep);
NumConstraint inry1(x,y,y<yr-lr/2);
NumConstraint outry1(x,y,y>yr-lr/2);
NumConstraint inry2(x,y,y>yr+lr/2);
NumConstraint outry2(x,y,y<yr+lr/2);
CtcFwdBwd incrx1(inrx1);
CtcFwdBwd incrx2(inrx2);
CtcFwdBwd incry1(inry1);
CtcFwdBwd incry2(inry2);
CtcFwdBwd outcrx1(outrx1);
CtcFwdBwd outcrx2(outrx2);
CtcFwdBwd outcry1(outry1);
CtcFwdBwd outcry2(outry2);
CtcUnion inrtemp(incrx1,incrx2,incry1);
CtcUnion inr1(inrtemp,incry2);
CtcCompo outrtemp(outcrx1,outcrx2,outcry1);
CtcCompo outr1(outrtemp,outcry2);
//2nd rectangle
xr = par->xr + wr/2;
NumConstraint inrx21(x,y,x>xr+ep);
NumConstraint outrx21(x,y,x<xr+ep);
NumConstraint inrx22(x,y,x<xr-ep);
NumConstraint outrx22(x,y,x>xr-ep);
NumConstraint inry21(x,y,y<yr-lr/2);
NumConstraint outry21(x,y,y>yr-lr/2);
NumConstraint inry22(x,y,y>yr+lr/2);
NumConstraint outry22(x,y,y<yr+lr/2);
CtcFwdBwd incrx21(inrx21);
CtcFwdBwd incrx22(inrx22);
CtcFwdBwd incry21(inry21);
CtcFwdBwd incry22(inry22);
CtcFwdBwd outcrx21(outrx21);
CtcFwdBwd outcrx22(outrx22);
CtcFwdBwd outcry21(outry21);
CtcFwdBwd outcry22(outry22);
CtcUnion inrtemp2(incrx21,incrx22,incry21);
CtcUnion inr2(inrtemp2,incry22);
CtcCompo outrtemp2(outcrx21,outcrx22,outcry21);
CtcCompo outr2(outrtemp2,outcry22);
//3nd rectangle top rectangle
yr=par->yr+par->lr/2;
xr=par->xr;
NumConstraint inrx31(x,y,x>xr+wr/2+ep);
NumConstraint outrx31(x,y,x<xr+wr/2+ep);
NumConstraint inrx32(x,y,x<xr-wr/2-ep);
NumConstraint outrx32(x,y,x>xr-wr/2-ep);
NumConstraint inry31(x,y,y<yr-ep);
NumConstraint outry31(x,y,y>yr-ep);
NumConstraint inry32(x,y,y>yr+ep);
NumConstraint outry32(x,y,y<yr+ep);
CtcFwdBwd incrx31(inrx31);
CtcFwdBwd incrx32(inrx32);
CtcFwdBwd incry31(inry31);
CtcFwdBwd incry32(inry32);
CtcFwdBwd outcrx31(outrx31);
CtcFwdBwd outcrx32(outrx32);
CtcFwdBwd outcry31(outry31);
CtcFwdBwd outcry32(outry32);
CtcUnion inrtemp3(incrx31,incrx32,incry31);
CtcUnion inr3(inrtemp3,incry32);
CtcCompo outrtemp3(outcrx31,outcrx32,outcry31);
CtcCompo outr3(outrtemp3,outcry32);
//4 rectangle bot
yr=par->yr-par->lr/2;
xr=par->xr;
NumConstraint inrx41(x,y,x>xr+wr/2+ep);
NumConstraint outrx41(x,y,x<xr+wr/2+ep);
NumConstraint inrx42(x,y,x<xr-wr/2-ep);
NumConstraint outrx42(x,y,x>xr-wr/2-ep);
NumConstraint inry41(x,y,y<yr-ep);
NumConstraint outry41(x,y,y>yr-ep);
NumConstraint inry42(x,y,y>yr+ep);
NumConstraint outry42(x,y,y<yr+ep);
CtcFwdBwd incrx41(inrx41);
CtcFwdBwd incrx42(inrx42);
CtcFwdBwd incry41(inry41);
CtcFwdBwd incry42(inry42);
CtcFwdBwd outcrx41(outrx41);
CtcFwdBwd outcrx42(outrx42);
CtcFwdBwd outcry41(outry41);
CtcFwdBwd outcry42(outry42);
CtcUnion inrtemp4(incrx41,incrx42,incry41);
CtcUnion inr4(inrtemp4,incry42);
CtcCompo outrtemp4(outcrx41,outcrx42,outcry41);
CtcCompo outr4(outrtemp4,outcry42);
CtcCompo inrtp(inr1,inr2,inr3);
CtcUnion outrtp(outr1,outr2,outr3);
CtcCompo inr(inrtp,inr4);
CtcUnion outr(outrtp,outr4);
yr = par->yr;
int maxq = 3; //nb of contractors
int Qinter = 2;
int ctcq = maxq - Qinter + 1; //nb for q-relaxed function of Ibex
Array<Ctc> inside1r1(inside1,inr,ain);
Array<Ctc> outside1r1(outside1,outr,aout);
Array<Ctc> inside2r1(inside2,inr,ain);
Array<Ctc> outside2r1(outside2,outr,aout);
Array<Ctc> inside3r1(inside3,inr,ain);
Array<Ctc> outside3r1(outside3,outr,aout);
CtcQInter outside1r(outside1r1,Qinter);
CtcQInter inside1r(inside1r1,ctcq);
CtcQInter outside2r(outside2r1,Qinter);
CtcQInter inside2r(inside2r1,ctcq);
CtcQInter outside3r(outside3r1,Qinter);
CtcQInter inside3r(inside3r1,ctcq);
// Build the initial box.
IntervalVector box(2);
box[0]=Interval(-10,10);
box[1]=Interval(-10,10);
par->vin.clear();
// Build the way boxes will be bisected.
// "LargestFirst" means that the dimension bisected
// is always the largest one.
int nbox1=0;
LargestFirst lf;
IntervalVector viinside1(2);
stack<IntervalVector> s;
s.push(box);
while (!s.empty()) {
IntervalVector box=s.top();
s.pop();
contract_and_draw(inside1r,box,viinside1,1,par,nbox1,Qt::magenta,Qt::red);
if (box.is_empty()) { continue; }
contract_and_draw(outside1r,box,viinside1,0,par,nbox1,Qt::darkBlue,Qt::cyan);
if (box.is_empty()) { continue; }
if (box.max_diam()<epsilon) {
R.DrawBox(box[0].lb(),box[0].ub(),box[1].lb(),box[1].ub(),QPen(Qt::yellow),QBrush(Qt::NoBrush));
} else {
pair<IntervalVector,IntervalVector> boxes=lf.bisect(box);
s.push(boxes.first);
s.push(boxes.second);
}
}
if(par->isinside==1){
robot_position_estimator(nbox1,par);
par->isinside1=1;
par->isinside=0;
//cout<<"area1: "<<par->area<<endl;
}
IntervalVector box2(2);
box2[0]=Interval(-10,10);
box2[1]=Interval(-10,10);
// Build the way boxes will be bisected.
// "LargestFirst" means that the dimension bisected
// is always the largest one.
int nbox2=0;
LargestFirst lf2;
IntervalVector viinside2(2);
stack<IntervalVector> s2;
s2.push(box2);
while (!s2.empty()) {
IntervalVector box2=s2.top();
s2.pop();
contract_and_draw(inside2r,box2,viinside2,2,par,nbox2,Qt::magenta,Qt::red);
if (box2.is_empty()) { continue; }
contract_and_draw(outside2r,box2,viinside2,0,par,nbox2,Qt::darkBlue,Qt::cyan);
if (box2.is_empty()) { continue; }
if (box2.max_diam()<epsilon) {
R.DrawBox(box2[0].lb(),box2[0].ub(),box2[1].lb(),box2[1].ub(),QPen(Qt::yellow),QBrush(Qt::NoBrush));
} else {
pair<IntervalVector,IntervalVector> boxes2=lf2.bisect(box2);
s2.push(boxes2.first);
s2.push(boxes2.second);
}
}
if(par->isinside==1){
robot_position_estimator(nbox2,par);
par->isinside2=1;
par->isinside=0;
//cout<<"area2: "<<par->area<<endl;
}
IntervalVector box3(2);
box3[0]=Interval(-10,10);
box3[1]=Interval(-10,10);
// Build the way boxes will be bisected.
// "LargestFirst" means that the dimension bisected
// is always the largest one.
int nbox3=0;
LargestFirst lf3;
IntervalVector viinside3(2);
stack<IntervalVector> s3;
s3.push(box3);
while (!s3.empty()) {
IntervalVector box3=s3.top();
s3.pop();
contract_and_draw(inside3r,box3,viinside3,3,par,nbox3,Qt::magenta,Qt::red);
if (box3.is_empty()) { continue; }
contract_and_draw(outside3r,box3,viinside3,0,par,nbox3,Qt::darkBlue,Qt::cyan);
if (box3.is_empty()) { continue; }
if (box3.max_diam()<epsilon) {
R.DrawBox(box3[0].lb(),box3[0].ub(),box3[1].lb(),box3[1].ub(),QPen(Qt::yellow),QBrush(Qt::NoBrush));
} else {
pair<IntervalVector,IntervalVector> boxes3=lf3.bisect(box3);
s3.push(boxes3.first);
s3.push(boxes3.second);
}
}
if(par->isinside==1){
robot_position_estimator(nbox3,par);
par->isinside3=1;
par->isinside=0;
//cout<<"area3: "<<par->area<<endl;
}
par->state.clear();
if (par->isinside1 ==1 || par->isinside2 ==1 || par->isinside3 ==1){
double *aimth = new double[3];
aimth[0] = get_angle(xb,yb,par->xin,par->yin)+M_PI ;
aimth[1] = get_angle(xb2,yb2,par->xin,par->yin)+M_PI;
aimth[2] = get_angle(xb3,yb3,par->xin,par->yin)+M_PI;
R.DrawLine(xb,yb,xb+r*cos(aimth[0]),yb+r*sin(aimth[0]),QPen(Qt::red));
R.DrawLine(xb2,yb2,xb2+r*cos(aimth[1]),yb2+r*sin(aimth[1]),QPen(Qt::red));
R.DrawLine(xb3,yb3,xb3+r*cos(aimth[2]),yb3+r*sin(aimth[2]),QPen(Qt::red));
par->state = std::string("found");
double kp = par->kp;
double u[3];
for (int i=0;i<3;i++){
u[i] = -kp*atan(tan((par->th[i] - (aimth[i] - arc/2.0 ))/2));
if(u[i]>par->sonar_speed) par->th[i] += par->sonar_speed;
if(u[i]<-par->sonar_speed) par->th[i] += -par->sonar_speed;
else par->th[i] += u[i];
}
// for (int i=0;i<3;i++){
// u[i] = atan(tan((par->th[i] - (aimth[i] - arc/2.0 ))/2));
// par->th[i] -=u[i];
// }
}
r = sqrt(r);
//cout<<"th1"<<th1<<endl;
R.DrawEllipse(xb,yb,par->ei,QPen(Qt::black),QBrush(Qt::NoBrush));
R.DrawEllipse(xb2,yb2,par->ei,QPen(Qt::black),QBrush(Qt::NoBrush));
R.DrawEllipse(xb3,yb3,par->ei,QPen(Qt::black),QBrush(Qt::NoBrush));
R.DrawLine(xb,yb,xb+r*cos(th2),yb+r*sin(th2),QPen(Qt::green));
R.DrawLine(xb2,yb2,xb2+r*cos(th22),yb2+r*sin(th22),QPen(Qt::green));
R.DrawLine(xb3,yb3,xb3+r*cos(th32),yb3+r*sin(th32),QPen(Qt::green));
R.DrawLine(xb,yb,xb+r*cos(th1),yb+r*sin(th1),QPen(Qt::green));
R.DrawLine(xb2,yb2,xb2+r*cos(th21),yb2+r*sin(th21),QPen(Qt::green));
R.DrawLine(xb3,yb3,xb3+r*cos(th31),yb3+r*sin(th31),QPen(Qt::green));
R.DrawEllipse(par->xa,par->ya,par->ra,QPen(Qt::black),QBrush(Qt::NoBrush));
R.DrawRobot(xr-wr/2,yr+lr/2,-3.14/2,wr,lr);
R.Save("paving");
par->vin.clear();
}
/*! Checks to see if the given AP is identical to itself ([this]). It also contains
some useful points of instrumentation for benchmarking table and usage characteristics.
\return TRUE, if and only if we match the AP given, false otherwise.
*/
bool PP_AttrProp::isExactMatch(const PP_AttrProp * pMatch) const
{
// The counters below are used in testing to profile call and chksum characteristics,
// including collision rates.
// NB: I'm not sure this initialization block is in the correct place.
#ifdef PT_TEST
static UT_uint32 s_Calls = 0;
static UT_uint32 s_PassedCheckSum = 0;
static UT_uint32 s_Matches = 0;
#endif
#ifdef PT_TEST
s_Calls++;
#endif
UT_return_val_if_fail (pMatch, false);
//
// Why is this here? Nothing is being changed?
// UT_return_val_if_fail (m_bIsReadOnly && pMatch->m_bIsReadOnly, false);
if (m_checkSum != pMatch->m_checkSum)
return false;
#ifdef PT_TEST
s_PassedCheckSum++;
#endif
UT_uint32 countMyAttrs = ((m_pAttributes) ? m_pAttributes->size() : 0);
UT_uint32 countMatchAttrs = ((pMatch->m_pAttributes) ? pMatch->m_pAttributes->size() : 0);
if (countMyAttrs != countMatchAttrs)
return false;
UT_uint32 countMyProps = ((m_pProperties) ? m_pProperties->size() : 0);
UT_uint32 countMatchProps = ((pMatch->m_pProperties) ? pMatch->m_pProperties->size() : 0);
if (countMyProps != countMatchProps)
return false;
if (countMyAttrs != 0)
{
UT_GenericStringMap<gchar*>::UT_Cursor ca1(m_pAttributes);
UT_GenericStringMap<gchar*>::UT_Cursor ca2(pMatch->m_pAttributes);
const gchar * v1 = ca1.first();
const gchar * v2 = ca2.first();
do
{
const gchar *l1 = ca1.key().c_str();
const gchar *l2 = ca2.key().c_str();
if (strcmp(l1, l2) != 0)
return false;
l1 = v1;
l2 = v2;
if (strcmp(l1,l2) != 0)
return false;
v1 = ca1.next();
v2 = ca2.next();
} while (ca1.is_valid() && ca2.is_valid());
}
if (countMyProps > 0)
{
UT_GenericStringMap<PropertyPair*>::UT_Cursor cp1(m_pProperties);
UT_GenericStringMap<PropertyPair*>::UT_Cursor cp2(pMatch->m_pProperties);
const PropertyPair* v1 = cp1.first();
const PropertyPair* v2 = cp2.first();
do
{
const gchar *l1 = cp1.key().c_str();
const gchar *l2 = cp2.key().c_str();
if (strcmp(l1, l2) != 0)
return false;
l1 = v1->first;
l2 = v2->first;
if (strcmp(l1,l2) != 0)
return false;
v1 = cp1.next();
v2 = cp2.next();
} while (cp1.is_valid() && cp2.is_valid());
#ifdef PT_TEST
s_Matches++;
#endif
}
return true;
}
int main(int argc, char *argv[])
{
// Initialize POOMA and output stream, using Tester class
Pooma::initialize(argc, argv);
Pooma::Tester tester(argc, argv);
tester.out() << argv[0] << ": Paws Field send/receive test B" << std::endl;
tester.out() << "--------------------------------------------" << std::endl;
#if POOMA_PAWS
// Some scalars to send and receive
int s1 = 1, origs1 = 1;
double s2 = 2.5, origs2 = 2.5;
int iters = 0, citers = 10;
// Fields to send and receive ... use different layouts in the two
// test codes.
Loc<2> blocks(2,1);
Interval<2> domain(6,2);
Interval<2> subdomain(3, 2);
Vector<2,double> origin(2.0);
Vector<2,double> spacings(0.2);
RectilinearMesh<2> mesh(domain, origin, spacings);
// Create the geometry and layout.
typedef DiscreteGeometry<Vert, RectilinearMesh<2> > Geometry_t;
Geometry_t geom(mesh);
GridLayout<2> layout(domain, blocks, ReplicatedTag());
// Now create the Fields
Field<Geometry_t, float, Brick> a1(geom);
Field<Geometry_t, int, MultiPatch<GridTag,Brick> > a2(geom, layout);
Array<2, float, Brick> a3(subdomain);
Array<2, float, Brick> ca1(domain);
Array<2, int, Brick> ca2(domain);
Array<2, float, Brick> ca3(subdomain);
// Initialize the arrays to be all zero, and the compare arrays to be
// what we expect from the sender.
a1 = 0;
a2 = 0;
a3 = 0;
ca1 = 10 * (iota(domain).comp(1) + 1) + iota(domain).comp(0) + 1;
ca2 = ca1 + 1000;
ca3 = ca1(subdomain);
// Create a Paws connection
tester.out() << "Creating PawsConnection object ..." << std::endl;
Connection<Paws> *paws = new Connection<Paws>("test6", argc, argv);
tester.out() << "Finished creating PawsConnection object." << std::endl;
// Establish connections for the two scalars
tester.out() << "Connecting s1 = " << s1 << " for input ..." << std::endl;
ConnectorBase *s1p = paws->connectScalar("s1", s1, ConnectionBase::in);
tester.out() << "Connecting s2 = " << s2 << " for output ..." << std::endl;
ConnectorBase *s2p = paws->connectScalar("s2", s2, ConnectionBase::out);
tester.out() << "Connecting iters = " << iters << " for input ...";
tester.out() << std::endl;
ConnectorBase *iterp=paws->connectScalar("iters", iters, ConnectionBase::in);
// Establish connections for the two fields; also connect up a view of
// the first fields
tester.out() << "Connecting a1 = " << a1 << " for input ..." << std::endl;
paws->connect("a1", a1, ConnectionBase::in);
tester.out() << "Connecting a2 = " << a2 << " for input ..." << std::endl;
paws->connect("a2", a2, ConnectionBase::in);
tester.out() << "Connecting a3 = " << a3 << " for input ..." << std::endl;
paws->connect("a1view", a3, ConnectionBase::in);
// Wait for everything to be ready to proceed
tester.out() << "Waiting for ready signal ..." << std::endl;
paws->ready();
tester.out() << "Ready complete, moving on." << std::endl;
// Modify s1, and update
s1 *= 2;
tester.out() << "Updating current s1 = " << s1 << " and s2 = " << s2;
tester.out() << " ..." << std::endl;
paws->update();
// Report the results
tester.out() << "Received update. New values:" << std::endl;
tester.out() << " s1 = " << s1 << " (should be " << origs1 << ")\n";
tester.out() << " s2 = " << s2 << " (should be " << origs2 << ")\n";
tester.out() << " iters = " << iters << " (should be " << citers << ")\n";
tester.out() << std::endl;
tester.check("s1 OK", s1 == origs1);
tester.check("s2 OK", s2 == origs2);
tester.check("iters OK", iters == citers);
// Also report Field results
tester.out() << "Received Fields as well. New values:" << std::endl;
tester.out() << " a1 = " << a1 << std::endl;
tester.out() << " a2 = " << a2 << std::endl;
tester.out() << " a3 = " << a3 << std::endl;
tester.check("a1 OK", all(a1.array() == ca1));
tester.check("a2 OK", all(a2.array() == ca2));
tester.check("a3 OK", all(a3 == ca3));
// Disconnect the scalars
int connections = paws->size();
tester.out() << "Disconnecting scalars ..." << std::endl;
paws->disconnect(s1p);
paws->disconnect(s2p);
paws->disconnect(iterp);
tester.check("3 less connections", paws->size() == (connections - 3));
// Do, in a loop, updates of the receiver. Add one to the arrays each time.
int runiters = iters;
while (runiters-- > 0)
{
ca1 += 1;
ca2 += 1;
ca3 += 1;
tester.out() << "Receiving for iters = " << iters << std::endl;
paws->update();
tester.out() << "Receive complete." << std::endl;
tester.check("a1 OK", all(a1.array() == ca1));
tester.check("a2 OK", all(a2.array() == ca2));
tester.check("a3 OK", all(a3 == ca3));
}
// Delete PAWS connection, disconnecting us from the other code.
tester.out() << "Deleting Connection<Paws> object ..." << std::endl;
delete paws;
#else // POOMA_PAWS
tester.out() << "Please configure with --paws to use this test code!"
<< std::endl;
#endif // POOMA_PAWS
// Finish up and report results
tester.out() << "-------------------------------------------" << std::endl;
int retval = tester.results("Paws Field send/receive test B");
Pooma::finalize();
return retval;
}
