SceneHandler::SceneHandler(std::shared_ptr<sf::RenderWindow> window)
: m_window(window)
, m_isPathActive(false)
, m_isObstacleActive(false)
, m_leader(nullptr)
{
m_unitTexture.loadFromFile("unit.png");
sf::RectangleShape shape1(sf::Vector2f(300.0f, 150.0f));
shape1.setPosition(150.0f, 400.0f);
shape1.setFillColor(sf::Color::Black);
m_obstacles.push_back(shape1);
/*sf::RectangleShape shape2(sf::Vector2f(120.0f, 200.0f));
shape2.setPosition(500.0f, 70.0f);
shape2.setFillColor(sf::Color::Black);
m_obstacles.push_back(shape2);*/
sf::RectangleShape shape3(sf::Vector2f(340.0f, 70.0f));
shape3.setPosition(800.0f, 500.0f);
shape3.setFillColor(sf::Color::Black);
m_obstacles.push_back(shape3);
m_ankers.push_back(createAnger(Vec2(-10.f, 30.0f)));
m_ankers.push_back(createAnger(Vec2(0.f, 50.0f)));
m_ankers.push_back(createAnger(Vec2(-50.f, 50.0f)));
m_ankers.push_back(createAnger(Vec2(-70.f, 30.0f)));
m_ankers.push_back(createAnger(Vec2(-70.f, 0.0f)));
m_ankers.push_back(createAnger(Vec2(-70.f, -30.0f)));
m_ankers.push_back(createAnger(Vec2(-50.f, -50.0f)));
}
void ScalarFiniteElement<D> ::
CalcDShape (const IntegrationPoint & ip,
FlatMatrixFixWidth<D> dshape) const
{
static bool firsttime = true;
if (firsttime)
{
cout << "WARNING: CalcDShape not overloaded for class, using numerical differentiation " << typeid(this).name() << ", ndof = " << ndof << endl;
firsttime = false;
}
int nd = GetNDof();
int sdim = D;
double eps = 2e-5;
ArrayMem<double, 100> hm1(nd), hm2(nd), hm3(nd), hm4(nd);
FlatVector<>
shape1(nd, &hm1[0]),
shape2(nd, &hm2[0]),
shape3(nd, &hm3[0]),
shape4(nd, &hm4[0]);
for (int i = 0; i < sdim; i++)
{
IntegrationPoint ip1 = ip;
IntegrationPoint ip2 = ip;
ip1(i) -= eps;
ip2(i) += eps;
CalcShape (ip1, shape1);
CalcShape (ip2, shape2);
ip1(i) -= eps;
ip2(i) += eps;
CalcShape (ip1, shape3);
CalcShape (ip2, shape4);
for (int j = 0; j < nd; j++)
dshape(j, i) =
2/(3*eps) * (shape2(j) - shape1(j))
-1/(12*eps) * (shape4(j) - shape3(j));
}
}
//
// Convex-Convex collision algorithm
//
void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
if (!m_manifoldPtr)
{
//swapped?
m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
m_ownManifold = true;
}
resultOut->setPersistentManifold(m_manifoldPtr);
#ifdef USE_BT_GJKEPA
btConvexShape* shape0(static_cast<btConvexShape*>(body0->getCollisionShape()));
btConvexShape* shape1(static_cast<btConvexShape*>(body1->getCollisionShape()));
const btScalar radialmargin(0/*shape0->getMargin()+shape1->getMargin()*/);
btGjkEpaSolver::sResults results;
if(btGjkEpaSolver::Collide( shape0,body0->getWorldTransform(),
shape1,body1->getWorldTransform(),
radialmargin,results))
{
dispatchInfo.m_debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
}
#else
btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
btGjkPairDetector::ClosestPointInput input;
//TODO: if (dispatchInfo.m_useContinuous)
m_gjkPairDetector.setMinkowskiA(min0);
m_gjkPairDetector.setMinkowskiB(min1);
input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
input.m_stackAlloc = dispatchInfo.m_stackAllocator;
// input.m_maximumDistanceSquared = btScalar(1e30);
input.m_transformA = body0->getWorldTransform();
input.m_transformB = body1->getWorldTransform();
m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#endif
if (m_ownManifold)
{
resultOut->refreshContactPoints();
}
}
void ThinPlateSplineShapeTransformerImpl::estimateTransformation(InputArray _pts1, InputArray _pts2,
std::vector<DMatch>& _matches )
{
Mat pts1 = _pts1.getMat();
Mat pts2 = _pts2.getMat();
CV_Assert((pts1.channels()==2) && (pts1.cols>0) && (pts2.channels()==2) && (pts2.cols>0));
CV_Assert(_matches.size()>1);
if (pts1.type() != CV_32F)
pts1.convertTo(pts1, CV_32F);
if (pts2.type() != CV_32F)
pts2.convertTo(pts2, CV_32F);
// Use only valid matchings //
std::vector<DMatch> matches;
for (size_t i=0; i<_matches.size(); i++)
{
if (_matches[i].queryIdx<pts1.cols &&
_matches[i].trainIdx<pts2.cols)
{
matches.push_back(_matches[i]);
}
}
// Organizing the correspondent points in matrix style //
Mat shape1((int)matches.size(),2,CV_32F); // transforming shape
Mat shape2((int)matches.size(),2,CV_32F); // target shape
for (int i=0, end = (int)matches.size(); i<end; i++)
{
Point2f pt1=pts1.at<Point2f>(0,matches[i].queryIdx);
shape1.at<float>(i,0) = pt1.x;
shape1.at<float>(i,1) = pt1.y;
Point2f pt2=pts2.at<Point2f>(0,matches[i].trainIdx);
shape2.at<float>(i,0) = pt2.x;
shape2.at<float>(i,1) = pt2.y;
}
shape1.copyTo(shapeReference);
// Building the matrices for solving the L*(w|a)=(v|0) problem with L={[K|P];[P'|0]}
//Building K and P (Neede to buil L)
Mat matK((int)matches.size(),(int)matches.size(),CV_32F);
Mat matP((int)matches.size(),3,CV_32F);
for (int i=0, end=(int)matches.size(); i<end; i++)
{
for (int j=0; j<end; j++)
{
if (i==j)
{
matK.at<float>(i,j)=float(regularizationParameter);
}
else
{
matK.at<float>(i,j) = distance(Point2f(shape1.at<float>(i,0),shape1.at<float>(i,1)),
Point2f(shape1.at<float>(j,0),shape1.at<float>(j,1)));
}
}
matP.at<float>(i,0) = 1;
matP.at<float>(i,1) = shape1.at<float>(i,0);
matP.at<float>(i,2) = shape1.at<float>(i,1);
}
//Building L
Mat matL=Mat::zeros((int)matches.size()+3,(int)matches.size()+3,CV_32F);
Mat matLroi(matL, Rect(0,0,(int)matches.size(),(int)matches.size())); //roi for K
matK.copyTo(matLroi);
matLroi = Mat(matL,Rect((int)matches.size(),0,3,(int)matches.size())); //roi for P
matP.copyTo(matLroi);
Mat matPt;
transpose(matP,matPt);
matLroi = Mat(matL,Rect(0,(int)matches.size(),(int)matches.size(),3)); //roi for P'
matPt.copyTo(matLroi);
//Building B (v|0)
Mat matB = Mat::zeros((int)matches.size()+3,2,CV_32F);
for (int i=0, end = (int)matches.size(); i<end; i++)
{
matB.at<float>(i,0) = shape2.at<float>(i,0); //x's
matB.at<float>(i,1) = shape2.at<float>(i,1); //y's
}
//Obtaining transformation params (w|a)
solve(matL, matB, tpsParameters, DECOMP_LU);
//tpsParameters = matL.inv()*matB;
//Setting transform Cost and Shape reference
Mat w(tpsParameters, Rect(0,0,2,tpsParameters.rows-3));
Mat Q=w.t()*matK*w;
transformCost=fabs(Q.at<float>(0,0)*Q.at<float>(1,1));//fabs(mean(Q.diag(0))[0]);//std::max(Q.at<float>(0,0),Q.at<float>(1,1));
tpsComputed=true;
}
void TestShapePainting::testPaintOrder()
{
// the stacking order determines the painting order so things on top
// get their paint called last.
// Each shape has a zIndex and within the children a container has
// it determines the stacking order. Its important to realize that
// the zIndex is thus local to a container, if you have layer1 and layer2
// with both various child shapes the stacking order of the layer shapes
// is most important, then within this the child shape index is used.
class OrderedMockShape : public MockShape {
public:
OrderedMockShape(QList<MockShape*> &list) : order(list) {}
void paint(QPainter &painter, const KViewConverter &converter) {
order.append(this);
MockShape::paint(painter, converter);
}
QList<MockShape*> ℴ
};
QList<MockShape*> order;
MockContainer top;
top.setZIndex(2);
OrderedMockShape shape1(order);
shape1.setZIndex(5);
OrderedMockShape shape2(order);
shape2.setZIndex(0);
top.addShape(&shape1);
top.addShape(&shape2);
MockContainer bottom;
bottom.setZIndex(1);
OrderedMockShape shape3(order);
shape3.setZIndex(-1);
OrderedMockShape shape4(order);
shape4.setZIndex(9);
bottom.addShape(&shape3);
bottom.addShape(&shape4);
MockCanvas canvas;
KShapeManager manager(&canvas);
manager.addShape(&top);
manager.addShape(&bottom);
QCOMPARE(manager.shapes().count(), 6);
QImage image(100, 100, QImage::Format_Mono);
QPainter painter(&image);
KViewConverter vc;
manager.paint(painter, vc, false);
QCOMPARE(top.paintedCount, 1);
QCOMPARE(bottom.paintedCount, 1);
QCOMPARE(shape1.paintedCount, 1);
QCOMPARE(shape2.paintedCount, 1);
QCOMPARE(shape3.paintedCount, 1);
QCOMPARE(shape4.paintedCount, 1);
QCOMPARE(order.count(), 4);
QVERIFY(order[0] == &shape3); // lowest first
QVERIFY(order[1] == &shape4);
QVERIFY(order[2] == &shape2);
QVERIFY(order[3] == &shape1);
// again, with clipping.
order.clear();
painter.setClipRect(0, 0, 100, 100);
manager.paint(painter, vc, false);
QCOMPARE(top.paintedCount, 2);
QCOMPARE(bottom.paintedCount, 2);
QCOMPARE(shape1.paintedCount, 2);
QCOMPARE(shape2.paintedCount, 2);
QCOMPARE(shape3.paintedCount, 2);
QCOMPARE(shape4.paintedCount, 2);
QCOMPARE(order.count(), 4);
QVERIFY(order[0] == &shape3); // lowest first
QVERIFY(order[1] == &shape4);
QVERIFY(order[2] == &shape2);
QVERIFY(order[3] == &shape1);
order.clear();
MockContainer root;
root.setZIndex(0);
MockContainer branch1;
branch1.setZIndex(1);
OrderedMockShape child1_1(order);
child1_1.setZIndex(1);
OrderedMockShape child1_2(order);
child1_2.setZIndex(2);
branch1.addShape(&child1_1);
branch1.addShape(&child1_2);
MockContainer branch2;
branch2.setZIndex(2);
OrderedMockShape child2_1(order);
child2_1.setZIndex(1);
OrderedMockShape child2_2(order);
child2_2.setZIndex(2);
branch2.addShape(&child2_1);
branch2.addShape(&child2_2);
root.addShape(&branch1);
root.addShape(&branch2);
QList<KShape*> sortedShapes;
sortedShapes.append(&root);
sortedShapes.append(&branch1);
sortedShapes.append(&branch2);
sortedShapes.append(branch1.shapes());
sortedShapes.append(branch2.shapes());
qSort(sortedShapes.begin(), sortedShapes.end(), KShape::compareShapeZIndex);
QCOMPARE(sortedShapes.count(), 7);
QVERIFY(sortedShapes[0] == &root);
QVERIFY(sortedShapes[1] == &branch1);
QVERIFY(sortedShapes[2] == &child1_1);
QVERIFY(sortedShapes[3] == &child1_2);
QVERIFY(sortedShapes[4] == &branch2);
QVERIFY(sortedShapes[5] == &child2_1);
QVERIFY(sortedShapes[6] == &child2_2);
}
static void TraceShape( // write an image file showing current shape on the image
const Shape& shape, // in: current search shape
const Image& pyrimg, // in: image scaled to this pyramid level
int ilev, // in: pyramid level (0 is full size)
int iter, // in: model iteration (-1 if start shape)
const char* suffix) // in
{
#if TRACE_IMAGES // will be 0 unless debugging (defined in stasm.h)
static int index; // number images so they appear in order in the directory
// start at index 30 because lower indices have already used for facedet etc.
if (strcmp(suffix, "start") == 0)
index = 30;
Image img; // pyrimg rescaled back to full size image (after rescaling by eyemouth)
const double RESOLUTION = 2; // 1 for no extra resolution, 2 for double resolution
const double rescale = RESOLUTION / GetPyrScale(ilev);
cv::resize(pyrimg, img, cv::Size(), rescale, rescale, cv::INTER_NEAREST);
CImage cimg; cvtColor(img, cimg, CV_GRAY2BGR); // color image
DesaturateImg(cimg);
Shape shape1(RoundMat(shape));
shape1 += .4; // put shape points in center of rescaled pixels
shape1 *= rescale;
DrawShape(cimg, shape1, C_YELLOW, false, 1);
char path[SLEN];
if (iter < 0) // start shape?
sprintf(path, "%s_%2.2d_%s.bmp", Base(imgpath_g), index, suffix);
else
sprintf(path, "%s_%2.2d_lev%d_iter%d_%s.bmp",
Base(imgpath_g), index, ilev, iter, suffix);
ImgPrintf(cimg, 10 * RESOLUTION, 20 * RESOLUTION, C_YELLOW, 2, path);
if (iter >= 0)
{
// draw 1D patch boundary at one point (patch is drawn
// horizontal, not rotated to shape boundary as it should be)
// [Thanks to Satish Lokkoju for RoundMat fix]
Shape shape2(RoundMat(shape));
int ipoint = 0;
int proflen = 9; // TASM_1D_PROFLEN
int x1 = cvRound(shape2(ipoint, IX)) - proflen / 2;
int x2 = x1 + proflen;
int y1 = cvRound(shape2(ipoint, IY));
int y2 = y1 + 1;
rectangle(cimg,
cv::Point(cvRound(rescale * x1), cvRound(rescale * y1)),
cv::Point(cvRound(rescale * x2), cvRound(rescale * y2)),
CV_RGB(255,0,0), 1);
// draw 2D patch boundary at one point
if (ilev <= HAT_START_LEV) // we use HATs only at upper pyr levs
{
// get position of left eye pupil by first converting to a shape17
ipoint = 0; // assume we can't get position of left eye pupil
Shape newshape(Shape17OrEmpty(shape2));
if (newshape.rows) // successfully converted to a shape17?
ipoint = L17_LPupil;
else
newshape = shape2;
#define round2(x) 2 * cvRound((x) / 2)
int patchwidth = HAT_PATCH_WIDTH + round2(ilev * HAT_PATCH_WIDTH_ADJ);
x1 = cvRound(newshape(ipoint, IX)) - patchwidth / 2;
x2 = x1 + patchwidth;
y1 = cvRound(newshape(ipoint, IY)) - patchwidth / 2;
y2 = y1 + patchwidth;
rectangle(cimg,
cv::Point(cvRound(rescale * x1), cvRound(rescale * y1)),
cv::Point(cvRound(rescale * x2), cvRound(rescale * y2)),
CV_RGB(255,0,0), 1);
}
}
lprintf("%s\n", path);
if (!cv::imwrite(path, cimg))
Err("Cannot write %s", path);
index++;
#endif // TRACE_IMAGES
}
