int main() {
Image<RGBColor> image;
string jpg_filenameL = stlplus::folder_up(string(THIS_SOURCE_DIR))
+ "/imageData/StanfordMobileVisualSearch/Ace_0.png";
ReadImage(jpg_filenameL.c_str(), &image);
Image<unsigned char> imageL;
Rgb2Gray(image, &imageL);
string jpg_filenameR = stlplus::folder_up(string(THIS_SOURCE_DIR))
+ "/imageData/StanfordMobileVisualSearch/Ace_1.png";
ReadImage(jpg_filenameR.c_str(), &image);
Image<unsigned char> imageR;
Rgb2Gray(image, &imageR);
// Define the used descriptor (SIFT : 128 float value)
typedef float descType;
typedef Descriptor<descType,128> SIFTDescriptor;
// Prepare vector to store detected feature and associated descriptor
vector<SIOPointFeature> featsL, featsR;
vector<SIFTDescriptor > descsL, descsR;
// Call SIFT detector
bool bOctaveMinus1 = true;
bool bRootSift = true;
SIFTDetector(imageL, featsL, descsL, bOctaveMinus1, bRootSift);
SIFTDetector(imageR, featsR, descsR, bOctaveMinus1, bRootSift);
// Show both images side by side
{
Image<unsigned char> concat;
ConcatH(imageL, imageR, concat);
string out_filename = "01_concat.jpg";
WriteImage(out_filename.c_str(), concat);
}
//- Draw features on the two image (side by side)
{
Image<unsigned char> concat;
ConcatH(imageL, imageR, concat);
//-- Draw features :
for (size_t i=0; i < featsL.size(); ++i ) {
const SIOPointFeature & imaA = featsL[i];
DrawCircle(imaA.x(), imaA.y(), imaA.scale(), 255, &concat);
}
for (size_t i=0; i < featsR.size(); ++i ) {
const SIOPointFeature & imaB = featsR[i];
DrawCircle(imaB.x()+imageL.Width(), imaB.y(), imaB.scale(), 255, &concat);
}
string out_filename = "02_features.jpg";
WriteImage(out_filename.c_str(), concat);
}
std::vector<IndMatch> vec_PutativeMatches;
//-- Perform matching -> find Nearest neighbor, filtered with Distance ratio
{
// Define the matcher
// and the used metric (Squared L2)
typedef L2_Vectorized<SIFTDescriptor::bin_type> Metric;
// Brute force matcher is defined as following:
typedef ArrayMatcherBruteForce<SIFTDescriptor::bin_type, Metric> MatcherT;
// Distance ratio quite high in order to have noise corrupted data. Squared due to squared metric
getPutativesMatches<SIFTDescriptor, MatcherT>(descsL, descsR, Square(0.8), vec_PutativeMatches);
// Draw correspondences after Nearest Neighbor ratio filter
svgDrawer svgStream( imageL.Width() + imageR.Width(), max(imageL.Height(), imageR.Height()));
svgStream.drawImage(jpg_filenameL, imageL.Width(), imageL.Height());
svgStream.drawImage(jpg_filenameR, imageR.Width(), imageR.Height(), imageL.Width());
for (size_t i = 0; i < vec_PutativeMatches.size(); ++i) {
//Get back linked feature, draw a circle and link them by a line
const SIOPointFeature & L = featsL[vec_PutativeMatches[i]._i];
const SIOPointFeature & R = featsR[vec_PutativeMatches[i]._j];
svgStream.drawLine(L.x(), L.y(), R.x()+imageL.Width(), R.y(), svgStyle().stroke("green", 2.0));
svgStream.drawCircle(L.x(), L.y(), L.scale(), svgStyle().stroke("yellow", 2.0));
svgStream.drawCircle(R.x()+imageL.Width(), R.y(), R.scale(),svgStyle().stroke("yellow", 2.0));
}
string out_filename = "03_siftMatches.svg";
ofstream svgFile( out_filename.c_str() );
svgFile << svgStream.closeSvgFile().str();
svgFile.close();
}
// Homography geometry filtering of putative matches
{
//A. get back interest point and send it to the robust estimation framework
Mat xL(2, vec_PutativeMatches.size());
Mat xR(2, vec_PutativeMatches.size());
for (size_t k = 0; k < vec_PutativeMatches.size(); ++k) {
const SIOPointFeature & imaL = featsL[vec_PutativeMatches[k]._i];
const SIOPointFeature & imaR = featsR[vec_PutativeMatches[k]._j];
xL.col(k) = imaL.coords().cast<double>();
xR.col(k) = imaR.coords().cast<double>();
}
//-- Homography robust estimation
std::vector<size_t> vec_inliers;
typedef ACKernelAdaptor<
openMVG::homography::kernel::FourPointSolver,
openMVG::homography::kernel::AsymmetricError,
UnnormalizerI,
Mat3>
KernelType;
KernelType kernel(
xL, imageL.Width(), imageL.Height(),
xR, imageR.Width(), imageR.Height(),
false); // configure as point to point error model.
Mat3 H;
std::pair<double,double> ACRansacOut = ACRANSAC(kernel, vec_inliers, 1024, &H,
std::numeric_limits<double>::infinity(),
true);
const double & thresholdH = ACRansacOut.first;
const double & NFAH = ACRansacOut.second;
// Check the homography support some point to be considered as valid
if (vec_inliers.size() > KernelType::MINIMUM_SAMPLES *2.5) {
std::cout << "\nFound a homography under the confidence threshold of: "
<< thresholdH << " pixels\n\twith: " << vec_inliers.size() << " inliers"
<< " from: " << vec_PutativeMatches.size()
<< " putatives correspondences"
<< std::endl;
//Show homography validated point and compute residuals
std::vector<double> vec_residuals(vec_inliers.size(), 0.0);
svgDrawer svgStream( imageL.Width() + imageR.Width(), max(imageL.Height(), imageR.Height()));
svgStream.drawImage(jpg_filenameL, imageL.Width(), imageL.Height());
svgStream.drawImage(jpg_filenameR, imageR.Width(), imageR.Height(), imageL.Width());
for ( size_t i = 0; i < vec_inliers.size(); ++i) {
size_t idx = vec_inliers[i];
const SIOPointFeature & LL = featsL[vec_PutativeMatches[vec_inliers[i]]._i];
const SIOPointFeature & RR = featsR[vec_PutativeMatches[vec_inliers[i]]._j];
const Vec2f L = LL.coords();
const Vec2f R = RR.coords();
svgStream.drawLine(L.x(), L.y(), R.x()+imageL.Width(), R.y(), svgStyle().stroke("green", 2.0));
svgStream.drawCircle(L.x(), L.y(), LL.scale(), svgStyle().stroke("yellow", 2.0));
svgStream.drawCircle(R.x()+imageL.Width(), R.y(), RR.scale(),svgStyle().stroke("yellow", 2.0));
// residual computation
vec_residuals[i] = std::sqrt(KernelType::ErrorT::Error(H,
LL.coords().cast<double>(),
RR.coords().cast<double>()));
}
string out_filename = "04_ACRansacHomography.svg";
ofstream svgFile( out_filename.c_str() );
svgFile << svgStream.closeSvgFile().str();
svgFile.close();
// Display some statistics of reprojection errors
float dMin, dMax, dMean, dMedian;
minMaxMeanMedian<float>(vec_residuals.begin(), vec_residuals.end(),
dMin, dMax, dMean, dMedian);
std::cout << std::endl
<< "Homography matrix estimation, residuals statistics:" << "\n"
<< "\t-- Residual min:\t" << dMin << std::endl
<< "\t-- Residual median:\t" << dMedian << std::endl
<< "\t-- Residual max:\t " << dMax << std::endl
<< "\t-- Residual mean:\t " << dMean << std::endl;
// Perform GUIDED MATCHING
// Use the computed model with all possible point couples
// and keep the one that have the error under the AC-RANSAC precision
// value.
Mat xL, xR;
PointsToMat(featsL, xL);
PointsToMat(featsR, xR);
std::vector<IndMatch> vec_corresponding_index;
GuidedMatching<Mat3, openMVG::homography::kernel::AsymmetricError>(
H, xL, xR, Square(thresholdH), vec_corresponding_index);
std::cout << "\nGuided homography matching found "
<< vec_corresponding_index.size() << " correspondences."
<< std::endl;
// Merge AC Ransac and H-Guided matches
std::set<IndMatch> set_matches(vec_corresponding_index.begin(),
vec_corresponding_index.end());
for ( size_t i = 0; i < vec_inliers.size(); ++i) {
set_matches.insert(vec_PutativeMatches[vec_inliers[i]]);
}
std::cout << "\nGuided homography + AC Ransac results matching found "
<< set_matches.size() << " correspondences."
<< std::endl;
// Update the corresponding index:
vec_corresponding_index.clear();
vec_corresponding_index.assign(set_matches.begin(), set_matches.end());
{
//Show homography validated correspondences and compute residuals
std::vector<double> vec_residuals(vec_corresponding_index.size(), 0.0);
svgDrawer svgStream( imageL.Width() + imageR.Width(), max(imageL.Height(), imageR.Height()));
svgStream.drawImage(jpg_filenameL, imageL.Width(), imageL.Height());
svgStream.drawImage(jpg_filenameR, imageR.Width(), imageR.Height(), imageL.Width());
for ( size_t i = 0; i < vec_corresponding_index.size(); ++i) {
const SIOPointFeature & LL = featsL[vec_corresponding_index[i]._i];
const SIOPointFeature & RR = featsR[vec_corresponding_index[i]._j];
const Vec2f L = LL.coords();
const Vec2f R = RR.coords();
svgStream.drawLine(L.x(), L.y(), R.x()+imageL.Width(), R.y(), svgStyle().stroke("green", 2.0));
svgStream.drawCircle(L.x(), L.y(), LL.scale(), svgStyle().stroke("yellow", 2.0));
svgStream.drawCircle(R.x()+imageL.Width(), R.y(), RR.scale(),svgStyle().stroke("yellow", 2.0));
// residual computation
vec_residuals[i] = std::sqrt(KernelType::ErrorT::Error(H,
LL.coords().cast<double>(),
RR.coords().cast<double>()));
}
string out_filename = "04_ACRansacHomography_guided.svg";
ofstream svgFile( out_filename.c_str() );
svgFile << svgStream.closeSvgFile().str();
svgFile.close();
// Display some statistics of reprojection errors
float dMin, dMax, dMean, dMedian;
minMaxMeanMedian<float>(vec_residuals.begin(), vec_residuals.end(),
dMin, dMax, dMean, dMedian);
std::cout << std::endl
<< "Homography matrix estimation, residuals statistics:" << "\n"
<< "\t-- Residual min:\t" << dMin << std::endl
<< "\t-- Residual median:\t" << dMedian << std::endl
<< "\t-- Residual max:\t " << dMax << std::endl
<< "\t-- Residual mean:\t " << dMean << std::endl;
}
}
else {
std::cout << "ACRANSAC was unable to estimate a rigid homography"
<< std::endl;
}
}
return EXIT_SUCCESS;
}
void RenderGrid::draw ()
{
_width = _cnvOpt.width;
_height = _cnvOpt.height;
_rc.fontsClear();
bool enableRefAtoms = refAtoms.size() > 0 && _factory.isItemMolecule(objs[0]);
if (enableRefAtoms && refAtoms.size() != objs.size())
throw Error("Number of reference atoms should be same as the number of objects");
bool enableTitles = titles.size() > 0;
if (enableTitles && titles.size() != objs.size())
throw Error("Number of titles should be same as the number of objects");
nRows = (objs.size() + nColumns - 1) / nColumns;
commentSize.set(0,0);
commentOffset = 0;
if (comment >= 0) {
_factory.getItem(comment).init();
_factory.getItem(comment).estimateSize();
commentSize.copy(_factory.getItem(comment).size);
commentOffset = _cnvOpt.commentOffset;
}
maxsz.set(0,0);
Vec2f refSizeLT, refSizeRB;
Array<float> columnExtentLeft, columnExtentRight, rowExtentTop, rowExtentBottom;
columnExtentLeft.clear_resize(nColumns);
columnExtentRight.clear_resize(nColumns);
columnExtentLeft.fill(0);
columnExtentRight.fill(0);
rowExtentTop.clear_resize(nRows);
rowExtentBottom.clear_resize(nRows);
rowExtentTop.fill(0);
rowExtentBottom.fill(0);
for (int i = 0; i < objs.size(); ++i) {
if (enableRefAtoms)
_factory.getItemMolecule(objs[i]).refAtom = refAtoms[i];
_factory.getItem(objs[i]).init();
_factory.getItem(objs[i]).setObjScale(_getObjScale(objs[i]));
_factory.getItem(objs[i]).estimateSize();
if (enableRefAtoms) {
const Vec2f& r = _factory.getItemMolecule(objs[i]).refAtomPos;
Vec2f d;
d.diff(_factory.getItemMolecule(objs[i]).size, r);
refSizeLT.max(r);
int col = i % nColumns;
int row = i / nColumns;
columnExtentLeft[col] = __max(columnExtentLeft[col], r.x);
columnExtentRight[col] = __max(columnExtentRight[col], d.x);
rowExtentTop[row] = __max(rowExtentTop[row], r.y);
rowExtentBottom[row] = __max(rowExtentBottom[row], d.y);
refSizeRB.max(d);
} else {
maxsz.max(_factory.getItem(objs[i]).size);
}
}
if (enableRefAtoms)
maxsz.sum(refSizeLT, refSizeRB);
maxTitleSize.set(0,0);
titleOffset = 0;
if (enableTitles) {
titleOffset = _cnvOpt.titleOffset;
for (int i = 0; i < titles.size(); ++i) {
_factory.getItem(titles[i]).init();
_factory.getItem(titles[i]).estimateSize();
maxTitleSize.max(_factory.getItem(titles[i]).size);
}
}
outerMargin.x = (float)(minMarg + _cnvOpt.marginX);
outerMargin.y = (float)(minMarg + _cnvOpt.marginY);
_width = __min(_width, _getMaxWidth());
_height = __min(_height, _getMaxHeight());
scale = _getScale(_width, _height);
if (_width < 1)
_width = _getDefaultWidth(scale);
if (_height < 1)
_height = _getDefaultHeight(scale);
_rc.initContext(_width, _height);
cellsz.set(__max(maxsz.x * scale, maxTitleSize.x),
maxsz.y * scale + maxTitleSize.y + titleOffset);
clientArea.set(cellsz.x * nColumns + _cnvOpt.gridMarginX * (nColumns - 1),
cellsz.y * nRows + _cnvOpt.gridMarginY * (nRows - 1));
_rc.init();
if (_cnvOpt.xOffset > 0 || _cnvOpt.yOffset > 0)
_rc.translate((float)_cnvOpt.xOffset, (float)_cnvOpt.yOffset);
_rc.translate(outerMargin.x, outerMargin.y);
if (_cnvOpt.commentPos == COMMENT_POS_TOP) {
_drawComment();
_rc.translate(0, (float)commentOffset);
}
_rc.storeTransform();
{
_rc.translate((_width - clientArea.x) / 2 - outerMargin.x, (_height - commentSize.y - commentOffset - clientArea.y) / 2 - outerMargin.y);
for (int i = 0; i < objs.size(); ++i) {
_rc.storeTransform();
{
int y = i / nColumns;
int x = i % nColumns;
Vec2f size(_factory.getItem(objs[i]).size);
_rc.translate(x * (cellsz.x + _cnvOpt.gridMarginX), y * (cellsz.y + _cnvOpt.gridMarginY));
_rc.storeTransform();
{
if (enableRefAtoms) {
_rc.translate(0.5f * (cellsz.x - (columnExtentRight[x] + columnExtentLeft[x]) * scale), 0.5f * (maxsz.y - (rowExtentBottom[y]+ rowExtentTop[y])) * scale);
const Vec2f r = _factory.getItemMolecule(objs[i]).refAtomPos;
_rc.translate((columnExtentLeft[x] - r.x) * scale, (rowExtentTop[y] - r.y) * scale);
} else {
_rc.translate(0.5f * (cellsz.x - size.x * scale), 0.5f * (maxsz.y - size.y) * scale);
}
_rc.scale(scale);
_factory.getItem(objs[i]).render();
}
_rc.restoreTransform();
_rc.removeStoredTransform();
_rc.translate(0, maxsz.y * scale + titleOffset);
if (enableTitles) {
Vec2f titleSize(_factory.getItem(titles[i]).size);
_rc.translate(_cnvOpt.titleAlign.getBboxRelativeOffset() * (cellsz.x - titleSize.x), 0.5f * (maxTitleSize.y - titleSize.y));
_factory.getItem(titles[i]).render();
}
}
_rc.restoreTransform();
_rc.removeStoredTransform();
}
}
_rc.restoreTransform();
_rc.removeStoredTransform();
if (_cnvOpt.commentPos == COMMENT_POS_BOTTOM) {
_rc.translate(0, _height - commentOffset - commentSize.y - 2*outerMargin.y);
_drawComment();
}
}
// React to size changes
void reshape(Vec2f Size, SimpleSceneManager *mgr)
{
mgr->resize(Size.x(), Size.y());
}
OGLText::OGLText(const Vec2f& position, const Color& color, const std::string& text, float size) :_position(position.X(), position.Y(), -1.f), m_text(text), _fontLoc("sansserif.glf"), _color(color)
{
m_font = new GLFont();
m_font->Create(_fontLoc);
//Approximation of size based on 12px 24px 22px
_size = size * (1 - (1 / m_font->m_height));
_width = m_font->m_width * text.size();
_rot = 0.f;
}
void SerialTestApp::mouseDrag( MouseEvent event )
{
mousePos = event.getPos();
Vec2f offset = mousePos - getWindowCenter();
float speed = offset.length();
offset.normalize();
float amtLeftWheel = 0;
float amtRightWheel = 0;
// Turning scheme:
// 0-90°
// 0 == other wheel moves forward @ speed
// 90 == other wheel moves backwards @ speed
// Never account for moving backwards.
// Hard left or right is all we can do.
float yRange = (MAX((offset.y*-1), 0.0)*2.0f) - 1.0f; // -1..1
//yRange*=-1;
// Always having one wheel moving forward ensures we're
// driving forward. We can't drive backwards.
if(offset.x < 0){
amtRightWheel = 1;
amtLeftWheel = yRange;// -1..1 //offset.y*-1;
}else{
amtLeftWheel = 1;
amtRightWheel = yRange;// -1..1 //offset.y*-1;
}
// Making the lw / rw amount a function of the speed
const static int MAX_SPEED = 200;
float speedScalar = MIN((speed/(float)MAX_SPEED), 1.0);
amtLeftWheel *= speedScalar;
amtRightWheel *= speedScalar;
int lw = 255+(amtLeftWheel*255); // 0..255..500
int rw = 255+(amtRightWheel*255); // 0..255..500
string directions = "" + boost::lexical_cast<string>((int)lw) + "," +
boost::lexical_cast<string>((int)rw) + ",\n";
console() << directions << "\n";
serial.writeString(directions);
// SIM Arduino code
long val = (lw*(long)1000)+rw;
int rVal = val % 1000;
int lVal = (val - rVal) * 0.001;
int lDirection = lVal >= 255 ? 1 : -1;
int rDirection = rVal >= 255 ? 1 : -1;
int lAbsVal = abs(lVal-255);
int rAbsVal = abs(rVal-255);
console() << "lw : " << lw << " rw: " << rw;
console() << " lAbsVal : " << lAbsVal << " rAbsVal: " << rAbsVal << "\n";
_ardLDir = lDirection;
_ardRDir = rDirection;
_ardLVal = lAbsVal;
_ardRVal = rAbsVal;
}
void Draw::mouseReleaseEvent( QMouseEvent *e ) {
// printf("mouse release event %d\n", e->button());
// draw
_mousePressed = 0;
if (_tool == D_DRAW && _currDPath) { // draw
//_pressure = 2.*_sens;
if (_currDPath->getNumElements() > 1) {
_parent->setCursor(Qt::WaitCursor);
Vec2f loc = unproject(e->x(), e->y(), _h);
_currDPath->addVertex(loc.x(), loc.y(), mapPressure());
_dc->addPath(_currDPath);
//for (int i=0; i<2; i++)
//_currDPath->fair();
_currDPath->resample(NULL);
_currDPath->redoStroke();
bool res=false;
if (!_freeDraw)
res = _currDPath->correspondRoto2(_is->getRotoCurves(_frame), _w, _h);
_currDPath->calculateStrokeDisplayList();
if (res) {
_corrShow = 1;
propagatePathEverywhere(_currDPath);
}
else _currDPath = NULL;
emit restoreCursor();
}
else {
delete _currDPath;
_currDPath = NULL;
}
}
/*
else if (_tool == D_PAINT && _currDPath) { // paint
Bboxf2D bbox = _currDPath->calcBbox();
DrawPatch *dch = new DrawPatch((int)floor(bbox.lower.x()),(int)floor(bbox.lower.y()),
(int)ceil(bbox.width()), (int)ceil(bbox.height()),
_currColor);
dch->takeStroke(_currDPath->giveStroke());
_currDPath->forgetStroke();
_dc->addPatch(dch);
//printf("patch corner: %f %f, window: %f %f\n",dch->x(), dch->y(),
// bbox.width(), bbox.height());
delete _currDPath;
_currDPath = NULL;
if (_propPatch && _propMode==1)
keyframeInterpolatePatch(dch);
}
*/
else if (_tool == D_SELECT) { // select
if (_selected && _dragDirty) {
regeneratePath(_selected);
_selected->fixLoc();
_dragDirty = false;
}
}
else if (_tool == D_OVERDRAW && _currDPath) {// overdraw
if (_currDPath->getNumElements() > 1) {
_currDPath->resample(NULL);
spliceDrawCurve();
}
else {
delete _currDPath; _currDPath=NULL;
_spliceFlag=-1;
}
}
else if (_tool==D_DROPPER) {
_captureColor = true;
_captureColorLoc.Set(e->x(), _h-e->y());
_parent->updateGL();
_captureColor = false;
}
else if (_tool==D_NUDGE) { // nudge
_spliceIndex=-1;
}
//_minimalRenderMode = 0;
_parent->updateGL();
}
void SpringCam::dragCam( const Vec2f &posOffset, float distFromCenter )
{
mEyeNode.mAcc += Vec3f( posOffset.xy(), distFromCenter );
}
//----------------------------------------------------------------------
// the next edge normal is computed as a pi/2 clockwise rotation of
// the direction from this point to the next one. The edge normals
// do thus point to the left of the path.
// Author: afischle
//----------------------------------------------------------------------
static Vec2f computeEdgeNormal(const Vec2f &a, const Vec2f &b, bool cw)
{
Vec2f d = b - a;
d.normalize();
return cw == true ? Vec2f(d.y(), -d.x()) : Vec2f(-d.y(), d.x());
}
void StageSelect::drawScrollbar() {
Vec2f pos = Vec2f(-15, -50);
Vec2f size = Vec2f(870, 760);
drawCenterTexture(pos,size, scrollbar_tex_);
drawCenterTexture(pos+Vec2f(-22,0), Vec2f(size.x()*0.2,size.y()*0.6), scroll_tex_);
}
float Boid::dist(Vec2f v1,Vec2f v2)
{
return v1.distance(v2);
}
void update(float dt, vector<Planet*>& planets, paramStruct& params)
{
if(charging && charge < MAXCHARGE) charge += dt * 1.5f;
pars = params;
if(jumpInert > .0f)
jumpInert-=dt;
if(jumpInert < .0f)
jumpInert = .0f;
if(turn != params.turn)
{
charging = false;
charge = .0f;
}
turn = params.turn;
pos += vel * dt;
if(pos.x > getWindowWidth())
{
pos.x = pos.x-getWindowWidth();
}
if(pos.x < 0)
{
pos.x = getWindowWidth()+pos.x;
}
if(pos.y > getWindowHeight())
{
pos.y = pos.y-getWindowHeight();
}
if(pos.y < 0)
{
pos.y = getWindowHeight()+pos.y;
}
Planet* closest = planets[0];
float dst = 999999999.0f;
vector<Planet*>::iterator it;
for(it = planets.begin(); it < planets.end(); it++)
{
Vec2f moonToPlanet = ((*it)->pos - pos);
if(moonToPlanet.length() < dst)
{
closest = (*it);
dst = moonToPlanet.length();
}
// vel += moonToPlanet.normalized() * 1.0f * (*it)->radius * (*it)->radius * 3.14 / (math<float>::max(moonToPlanet.length() * moonToPlanet.length(), 80.0f));
// vel += moonToPlanet.normalized() * params.distFactor * moonToPlanet.length();//20000.0f/(math<float>::max(moonToPlanet.length(), 100.0f));
// vel += (-params.repulse)* moonToPlanet.normalized() / moonToPlanet.length();
}
nearest = closest;
Vec2f moonToPlanet = closest->pos - pos;
if(!closestPlanet && moonToPlanet.length() > closest->radius * 2.5f)
{
vel += moonToPlanet.normalized() * 36.5f * pars.speed *
closest->radius * closest->radius * closest->radius * (2.0f/3.0f) * M_PI
/ (math<float>::max(moonToPlanet.length() * moonToPlanet.length(), 80.0f));
}
if(!jumpInert && !closestPlanet && moonToPlanet.length() < closest->radius * 1.7f && losing.size() == 0)
{
closestPlanet = closest;
}
if(closestPlanet && !jumpInert && losing.size() == 0)
{
Vec2f m2cp = (closestPlanet->pos - pos);
Vec2f r = pos - closestPlanet->pos;
r.normalize();
r.rotate(M_PI/1.97f);
vel = r * 300.0f * pars.speed;
if(m2cp.length() < closestPlanet->radius)
{
pos += -m2cp.normalized() * (5.0f + closestPlanet->radius - m2cp.length());
}
}
//vel /= planets.size();
//
// Vec2f newpos = (pos + vel * dt);
// for(it = planets.begin(); it < planets.end(); it++)
// {
// if(newpos.distance((*it)->pos) < (*it)->radius * 1.5f)
// vel -= ((*it)->pos - pos);
// }
closest = 0;
delete closest;
// particles
vector<Particle*>::iterator pit;
for(pit = losing.begin(); pit < losing.end(); pit++)
{
(*pit)->update(dt);
if((*pit)->time > (*pit)->lt)
losing.erase(pit);
}
}
Vec2f MoleculeLayoutSmoothingSegment::_getPosition(Vec2f p) {
Vec2f point;
point.copy(p);
point.rotate(_finish - _start);
return point + _start;
}
MoleculeLayoutSmoothingSegment::MoleculeLayoutSmoothingSegment(MoleculeLayoutGraphSmart& mol, Vec2f& start, Vec2f& finish) :
_graph(mol),
_start(start),
_finish(finish)
{
_center.zero();
Vec2f diameter = (_finish - _start);
_length = diameter.length();
Vec2f rotate_vector = diameter / diameter.lengthSqr();
rotate_vector.y *= -1;
_pos.clear_resize(_graph.vertexEnd());
bool is_line = false;
for (int v : _graph.vertices()) if (_graph.getVertex(v).degree() == 1) is_line = true;
if (!is_line) {
for (int v : _graph.vertices()) {
_pos[v].copy(_graph.getPos(v));
_pos[v] -= _start;
_pos[v].rotate(rotate_vector);
}
}
else {
// this is straight line
QS_DEF(Array<int>, vert);
vert.clear(); // list or vertices in order of connection
for (int v : _graph.vertices()) if (_graph.getVertex(v).degree() == 1) {
vert.push(v);
break;
}
vert.push(_graph.getVertex(vert[0]).neiVertex(_graph.getVertex(vert[0]).neiBegin()));
while (vert.size() < _graph.vertexCount()) {
for (int n = _graph.getVertex(vert.top()).neiBegin(); n != _graph.getVertex(vert.top()).neiEnd(); n = _graph.getVertex(vert.top()).neiNext(n))
if (_graph.getVertex(vert.top()).neiVertex(n) != vert[vert.size() - 2]) {
vert.push(_graph.getVertex(vert.top()).neiVertex(n));
}
}
for (int i = 0; i < vert.size(); i++) _pos[vert[i]].set(i * 1. / (vert.size() - 1), 0);
}
// double ternary search of center of component
float MLx = 0, Ly = 0, MRx = 0, Ry = 0, Lx, Rx;
for (int v : _graph.vertices()) {
MLx = __min(MLx, _pos[v].x);
MRx = __max(MRx, _pos[v].x);
Ly = __min(Ly, _pos[v].y);
Ry = __max(Ry, _pos[v].y);
}
while (Ry - Ly > EPSILON) {
float dy = (Ry - Ly) / 3;
float ry[2];
float My = Ly + dy;
for (int i = 0; i < 2; i++) {
Lx = MLx, Rx = MRx;
float rx[2];
while (Rx - Lx > EPSILON) {
float dx = (Rx - Lx) / 3;
float Mx = Lx + dx;
for (int j = 0; j < 2; j++) {
rx[j] = calc_radius(Vec2f(Mx, My));
Mx += dx;
}
if (rx[0] > rx[1]) Lx += dx; else Rx -= dx;
}
ry[i] = calc_radius(Vec2f(Rx, My));
My += dy;
}
if (ry[0] > ry[1]) Ly += dy; else Ry -= dy;
}
_center = Vec2f(Rx, Ry);
_radius = calc_radius(_center);
/*
_radius = 0;
Vec2f center(0.5, 0);
for (int v : _graph.vertices()) {
float dist = (center - _pos[v]).length();
if (dist > _radius) _radius = dist;
}
_center = center;*/
_square = 0;
}
void BorderLayout::updateLayout(const MFUnrecChildComponentPtr* Components, const Component* ParentComponent) const
{
Pnt2f borderTopLeft, borderBottomRight;
dynamic_cast<const ComponentContainer*>(ParentComponent)->getInsideInsetsBounds(borderTopLeft, borderBottomRight);
Vec2f borderSize(borderBottomRight-borderTopLeft);
Real32 NorthHeight(0);
Real32 SouthHeight(0);
Real32 WestWidth(0);
Real32 EastWidth(0);
Vec2f size;
Vec2f offset;
// the first pass through gets some crucial dimensions to determine
// the sizes of the buttons
for(UInt32 i = 0 ; i<Components->size(); ++i)
{
if((*Components)[i]->getConstraints() != NULL)
{
switch (dynamic_cast<BorderLayoutConstraints*>((*Components)[i]->getConstraints())->getRegion()) {
// don't need to do anything with the center quite yet
case BorderLayoutConstraints::BORDER_NORTH:
NorthHeight = (*Components)[i]->getPreferredSize().y();
break;
case BorderLayoutConstraints::BORDER_EAST:
EastWidth = (*Components)[i]->getPreferredSize().x();
break;
case BorderLayoutConstraints::BORDER_SOUTH:
SouthHeight = (*Components)[i]->getPreferredSize().y();
break;
case BorderLayoutConstraints::BORDER_WEST:
WestWidth = (*Components)[i]->getPreferredSize().x();
break;
default:
break;
}
}
}
Pnt2f Pos;
// this second pass sets its size and draws them
for(UInt32 i = 0 ; i<Components->size(); ++i)
{
// Find its region and draw it accordingly
if((*Components)[i]->getConstraints() != NULL)
{
switch (dynamic_cast<BorderLayoutConstraints*>((*Components)[i]->getConstraints())->getRegion()) {
case BorderLayoutConstraints::BORDER_CENTER:
// set up the size of the button and its extra displacement
if ((*Components)[i]->getMaxSize().x() < borderSize.x()-(WestWidth+EastWidth))
{
size[0] = (*Components)[i]->getMaxSize().x();
offset[0] = (borderSize.x()-(WestWidth+EastWidth)-size[0])/2 + WestWidth;
}
else
{
size[0] = borderSize.x()-(WestWidth+EastWidth);
offset[0] = WestWidth;
}
if ((*Components)[i]->getMaxSize().y() < borderSize.y()-(NorthHeight+SouthHeight))
{
size[1] = (*Components)[i]->getMaxSize().y();
offset[1] = (borderSize.y()-(NorthHeight+SouthHeight)-size[1])/2 + NorthHeight;
}
else
{
size[1] = borderSize.y()-(NorthHeight+SouthHeight);
offset[1] = NorthHeight;
}
break;
case BorderLayoutConstraints::BORDER_NORTH:
// set up the size of the button and its extra displacement
size[1] = (*Components)[i]->getPreferredSize().y();
offset[1] = 0;
if ((*Components)[i]->getMaxSize().x() < borderSize.x())
{
size[0] = (*Components)[i]->getMaxSize().x();
offset[0] = (borderSize.x()-size[0])/2;
}
else
{
size[0] = borderSize.x();
offset[0] = 0;
}
break;
case BorderLayoutConstraints::BORDER_EAST:
// set up the size of the button and its extra displacement
size[0] = (*Components)[i]->getPreferredSize().x();
offset[0] = borderSize.x()-size.x();
if ((*Components)[i]->getMaxSize().y() < borderSize.y()-(NorthHeight+SouthHeight))
{
size[1] = (*Components)[i]->getMaxSize().y();
offset[1] = (borderSize.y()-size[1]-(NorthHeight+SouthHeight))/2+NorthHeight;
}
else
{
size[1] = borderSize.y()-(NorthHeight+SouthHeight);
offset[1] = NorthHeight;
}
break;
case BorderLayoutConstraints::BORDER_SOUTH:
// set up the size of the button and its extra displacement
size[1] = (*Components)[i]->getPreferredSize().y();
offset[1] = borderSize.y()-size[1];
if ((*Components)[i]->getMaxSize().x() < borderSize.x())
{
size[0] = (*Components)[i]->getMaxSize().x();
offset[0] = (borderSize.x()-size[0])/2;
}
else
{
size[0] = borderSize.x();
offset[0] = 0;
}
break;
case BorderLayoutConstraints::BORDER_WEST:
// set up the size of the button and its extra displacement
size[0] = (*Components)[i]->getPreferredSize().x();
offset[0] = 0;
if ((*Components)[i]->getMaxSize().y() < borderSize.y()-(NorthHeight+SouthHeight))
{
size[1] = (*Components)[i]->getMaxSize().y();
offset[1] = (borderSize.y()-size[1]-(NorthHeight+SouthHeight))/2 + NorthHeight;
}
else
{
size[1] = borderSize.y()-(NorthHeight+SouthHeight);
offset[1] = NorthHeight;
}
break;
default:
// if it isn't any of the regions, set it up to not be drawn
size[0] = size[1] = offset[0] = offset[1] = 0;
break;
}
size[0] = osgMin(osgMax(size[0], (*Components)[i]->getMinSize().x()), (*Components)[i]->getMaxSize().x());
size[1] = osgMin(osgMax(size[1], (*Components)[i]->getMinSize().y()), (*Components)[i]->getMaxSize().y());
// now set the position and size of the button
if((*Components)[i]->getSize() != size)
{
(*Components)[i]->setSize(size);
}
Pos = borderTopLeft + Vec2f(offset);
if((*Components)[i]->getPosition() != Pos)
{
(*Components)[i]->setPosition(Pos);
}
}
}
}
int main() {
std::string sInputDir = stlplus::folder_up(string(THIS_SOURCE_DIR))
+ "/imageData/SceauxCastle/";
string jpg_filenameL = sInputDir + "100_7101.jpg";
string jpg_filenameR = sInputDir + "100_7102.jpg";
Image<unsigned char> imageL, imageR;
ReadImage(jpg_filenameL.c_str(), &imageL);
ReadImage(jpg_filenameR.c_str(), &imageR);
// Define the used descriptor (SIFT : 128 float value)
typedef float descType;
typedef Descriptor<descType,128> SIFTDescriptor;
// Prepare vector to store detected feature and associated descriptor
vector<SIOPointFeature> featsL, featsR;
vector<SIFTDescriptor > descsL, descsR;
// Call SIFT detector
bool bOctaveMinus1 = false;
bool bRootSift = true;
SIFTDetector(imageL, featsL, descsL, bOctaveMinus1, bRootSift);
SIFTDetector(imageR, featsR, descsR, bOctaveMinus1, bRootSift);
// Show both images side by side
{
Image<unsigned char> concat;
ConcatH(imageL, imageR, concat);
string out_filename = "01_concat.jpg";
WriteImage(out_filename.c_str(), concat);
}
//- Draw features on the two image (side by side)
{
Image<unsigned char> concat;
ConcatH(imageL, imageR, concat);
//-- Draw features :
for (size_t i=0; i < featsL.size(); ++i ) {
const SIOPointFeature & imaA = featsL[i];
DrawCircle(imaA.x(), imaA.y(), imaA.scale(), 255, &concat);
}
for (size_t i=0; i < featsR.size(); ++i ) {
const SIOPointFeature & imaB = featsR[i];
DrawCircle(imaB.x()+imageL.Width(), imaB.y(), imaB.scale(), 255, &concat);
}
string out_filename = "02_features.jpg";
WriteImage(out_filename.c_str(), concat);
}
std::vector<IndMatch> vec_PutativeMatches;
//-- Perform matching -> find Nearest neighbor, filtered with Distance ratio
{
// Define the matcher
// and the used metric (Squared L2)
typedef L2_Vectorized<SIFTDescriptor::bin_type> Metric;
// Brute force matcher is defined as following:
typedef ArrayMatcherBruteForce<SIFTDescriptor::bin_type, Metric> MatcherT;
// Distance ratio quite high in order to have noise corrupted data. Squared due to squared metric
getPutativesMatches<SIFTDescriptor, MatcherT>(descsL, descsR, Square(0.8), vec_PutativeMatches);
IndMatchDecorator<float> matchDeduplicator(
vec_PutativeMatches, featsL, featsR);
matchDeduplicator.getDeduplicated(vec_PutativeMatches);
// Draw correspondences after Nearest Neighbor ratio filter
svgDrawer svgStream( imageL.Width() + imageR.Width(), max(imageL.Height(), imageR.Height()));
svgStream.drawImage(jpg_filenameL, imageL.Width(), imageL.Height());
svgStream.drawImage(jpg_filenameR, imageR.Width(), imageR.Height(), imageL.Width());
for (size_t i = 0; i < vec_PutativeMatches.size(); ++i) {
//Get back linked feature, draw a circle and link them by a line
const SIOPointFeature & L = featsL[vec_PutativeMatches[i]._i];
const SIOPointFeature & R = featsR[vec_PutativeMatches[i]._j];
svgStream.drawLine(L.x(), L.y(), R.x()+imageL.Width(), R.y(), svgStyle().stroke("green", 2.0));
svgStream.drawCircle(L.x(), L.y(), L.scale(), svgStyle().stroke("yellow", 2.0));
svgStream.drawCircle(R.x()+imageL.Width(), R.y(), R.scale(),svgStyle().stroke("yellow", 2.0));
}
string out_filename = "03_siftMatches.svg";
ofstream svgFile( out_filename.c_str() );
svgFile << svgStream.closeSvgFile().str();
svgFile.close();
}
// Essential geometry filtering of putative matches
{
Mat3 K;
//read K from file
if (!readIntrinsic(stlplus::create_filespec(sInputDir,"K","txt"), K))
{
std::cerr << "Cannot read intrinsic parameters." << std::endl;
return EXIT_FAILURE;
}
//A. prepare the corresponding putatives points
Mat xL(2, vec_PutativeMatches.size());
Mat xR(2, vec_PutativeMatches.size());
for (size_t k = 0; k < vec_PutativeMatches.size(); ++k) {
const SIOPointFeature & imaL = featsL[vec_PutativeMatches[k]._i];
const SIOPointFeature & imaR = featsR[vec_PutativeMatches[k]._j];
xL.col(k) = imaL.coords().cast<double>();
xR.col(k) = imaR.coords().cast<double>();
}
//B. robust estimation of the essential matrix
std::vector<size_t> vec_inliers;
Mat3 E;
std::pair<size_t, size_t> size_imaL(imageL.Width(), imageL.Height());
std::pair<size_t, size_t> size_imaR(imageR.Width(), imageR.Height());
double thresholdE = 0.0, NFA = 0.0;
if (robustEssential(
K, K, // intrinsics
xL, xR, // corresponding points
&E, // essential matrix
&vec_inliers, // inliers
size_imaL, // Left image size
size_imaR, // Right image size
&thresholdE, // Found AContrario Theshold
&NFA, // Found AContrario NFA
std::numeric_limits<double>::infinity()))
{
std::cout << "\nFound an Essential matrix under the confidence threshold of: "
<< thresholdE << " pixels\n\twith: " << vec_inliers.size() << " inliers"
<< " from: " << vec_PutativeMatches.size()
<< " putatives correspondences"
<< std::endl;
// Show Essential validated point
svgDrawer svgStream( imageL.Width() + imageR.Width(), max(imageL.Height(), imageR.Height()));
svgStream.drawImage(jpg_filenameL, imageL.Width(), imageL.Height());
svgStream.drawImage(jpg_filenameR, imageR.Width(), imageR.Height(), imageL.Width());
for ( size_t i = 0; i < vec_inliers.size(); ++i) {
size_t idx = vec_inliers[i];
const SIOPointFeature & LL = featsL[vec_PutativeMatches[vec_inliers[i]]._i];
const SIOPointFeature & RR = featsR[vec_PutativeMatches[vec_inliers[i]]._j];
const Vec2f L = LL.coords();
const Vec2f R = RR.coords();
svgStream.drawLine(L.x(), L.y(), R.x()+imageL.Width(), R.y(), svgStyle().stroke("green", 2.0));
svgStream.drawCircle(L.x(), L.y(), LL.scale(), svgStyle().stroke("yellow", 2.0));
svgStream.drawCircle(R.x()+imageL.Width(), R.y(), RR.scale(),svgStyle().stroke("yellow", 2.0));
}
string out_filename = "04_ACRansacEssential.svg";
ofstream svgFile( out_filename.c_str() );
svgFile << svgStream.closeSvgFile().str();
svgFile.close();
//C. Extract the rotation and translation of the camera from the essential matrix
Mat3 R;
Vec3 t;
if (!estimate_Rt_fromE(K, K, xL, xR, E, vec_inliers,
&R, &t))
{
std::cerr << " /!\\ Failed to compute initial R|t for the initial pair" << std::endl;
return false;
}
std::cout << std::endl
<< "-- Rotation|Translation matrices: --" << std::endl
<< R << std::endl << std::endl << t << std::endl;
// Build Left and Right Camera
PinholeCamera camL(K, Mat3::Identity(), Vec3::Zero());
PinholeCamera camR(K, R, t);
//C. Triangulate and export inliers as a PLY scene
std::vector<Vec3> vec_3DPoints;
triangulateAndSaveResult(
camL, camR,
vec_inliers,
xL, xR, vec_3DPoints);
// Export as PLY (camera pos + 3Dpoints)
std::vector<Vec3> vec_camPos;
vec_camPos.push_back( camL._C );
vec_camPos.push_back( camR._C );
exportToPly(vec_3DPoints, vec_camPos, "EssentialGeometry.ply");
}
else {
std::cout << "ACRANSAC was unable to estimate a rigid essential matrix"
<< std::endl;
}
}
return EXIT_SUCCESS;
}
void StageSelect::drawWindow() {
Vec2f pos = Vec2f(-WIDTH / 2, -HEIGHT / 2);
Vec2f size = Vec2f(WIDTH, HEIGHT);
drawTextureBox(pos.x(), pos.y(), size.x(), size.y(),
0, 0, window_tex_.width(), window_tex_.height(), window_tex_, Color::white);
}
void World::simulate(float dt)
{
mAllocator.clearFrame();
int vIterNum = 50;
int pIterNum = 10;
mColManager.preocss( dt );
for( RigidBodyList::iterator iter = mRigidBodies.begin() ,itEnd = mRigidBodies.end();
iter != itEnd ; ++iter )
{
RigidBody* body = *iter;
body->saveState();
if ( body->getMotionType() != BodyMotion::eStatic )
{
if ( body->getMotionType() == BodyMotion::eDynamic )
body->addLinearImpulse( body->mMass * mGrivaty * dt );
body->applyImpulse();
body->mLinearVel *= 1.0 / ( 1 + body->mLinearDamping );
}
}
ContactManifold** sortedContact = new ( mAllocator ) ContactManifold* [ mColManager.mMainifolds.size() ];
int numMainfold = mColManager.mMainifolds.size();
int idxStatic = mColManager.mMainifolds.size() - 1;
int idxNormal = 0;
for( int i = 0 ; i < mColManager.mMainifolds.size() ; ++i )
{
ContactManifold& cm = *mColManager.mMainifolds[i];
RigidBody* bodyA = static_cast< RigidBody* >( cm.mContect.object[0] );
RigidBody* bodyB = static_cast< RigidBody* >( cm.mContect.object[1] );
if ( bodyA->getMotionType() == BodyMotion::eStatic ||
bodyB->getMotionType() == BodyMotion::eStatic )
{
sortedContact[idxStatic--] = &cm;
}
else
{
sortedContact[idxNormal++] = &cm;
}
}
for( int i = 0 ; i < numMainfold ; ++i )
{
ContactManifold& cm = *sortedContact[i];
Contact& c = cm.mContect;
Vec2f cp = 0.5 * ( c.pos[0] + c.pos[1] );
RigidBody* bodyA = static_cast< RigidBody* >( c.object[0] );
RigidBody* bodyB = static_cast< RigidBody* >( c.object[1] );
Vec2f vA = bodyA->getVelFromWorldPos( cp );
Vec2f vB = bodyB->getVelFromWorldPos( cp );
float vrel = c.normal.dot( vB - vA );
float relectParam = 1.0f;
cm.velParam = 0;
if ( vrel < -1 )
{
cm.velParam = -relectParam * vrel;
}
//cm.impulse = 0;
////warm start
Vec2f rA = cp - bodyA->mPosCenter;
Vec2f rB = cp - bodyB->mPosCenter;
Vec2f dp = cm.impulse * c.normal;
bodyA->mLinearVel -= dp * bodyA->mInvMass;
//bodyA->mAngularVel -= rA.cross( dp ) * bodyA->mInvI;
bodyB->mLinearVel += dp * bodyB->mInvMass;
//bodyB->mAngularVel += rB.cross( dp ) * bodyB->mInvI;
}
if ( numMainfold != 0 )
jumpDebug();
//std::sort( sortedContact.begin() , sortedContact.end() , DepthSort() );
for( int nIter = 0 ; nIter < vIterNum ; ++nIter )
{
for( int i = 0 ; i < numMainfold ; ++i )
{
ContactManifold& cm = *sortedContact[i];
Contact& c = cm.mContect;
RigidBody* bodyA = static_cast< RigidBody* >( c.object[0] );
RigidBody* bodyB = static_cast< RigidBody* >( c.object[1] );
Vec2f cp = 0.5 * ( c.pos[0] + c.pos[1] );
Vec2f vA = bodyA->getVelFromWorldPos( cp );
Vec2f vB = bodyB->getVelFromWorldPos( cp );
Vec2f rA = cp - bodyA->mPosCenter;
Vec2f rB = cp - bodyB->mPosCenter;
Vec2f vrel = vB - vA;
float vn = vrel.dot( c.normal );
float nrA = rA.cross( c.normal );
float nrB = rB.cross( c.normal );
float invMass = 0;
invMass += bodyA->mInvMass + bodyA->mInvI * nrA * nrA;
invMass += bodyB->mInvMass + bodyB->mInvI * nrB * nrB;
float impulse = -( vn - cm.velParam ) / invMass;
float newImpulse = Math::Max( cm.impulse + impulse , 0.0f );
impulse = newImpulse - cm.impulse;
bodyA->mLinearVel -= impulse * c.normal * bodyA->mInvMass;
//bodyA->mAngularVel -= impulse * nrA * bodyA->mInvI;
bodyB->mLinearVel += impulse * c.normal * bodyB->mInvMass;
//bodyB->mAngularVel += impulse * nrB * bodyB->mInvI;
cm.impulse = newImpulse;
float fa = impulse * nrA * bodyA->mInvI;
float fb = impulse * nrB * bodyB->mInvI;
if ( fa != 0 || fb !=0 )
{
int i = 1;
}
if ( numMainfold != 0 )
jumpDebug();
}
}
for( RigidBodyList::iterator iter = mRigidBodies.begin() ,itEnd = mRigidBodies.end();
iter != itEnd ; ++iter )
{
RigidBody* body = *iter;
body->intergedTramsform( dt );
//body->mAngularVel = 0;
}
for( int nIter = 0 ; nIter < pIterNum ; ++nIter )
{
float const kValueB = 0.8f;
float const kMaxDepth = 2.f;
float const kSlopValue = 0.0001f;
float maxDepth = 0.0;
for( int i = 0 ; i < numMainfold ; ++i )
{
ContactManifold& cm = *sortedContact[i];
Contact& c = cm.mContect;
RigidBody* bodyA = static_cast< RigidBody* >( c.object[0] );
RigidBody* bodyB = static_cast< RigidBody* >( c.object[1] );
if ( bodyB->getMotionType() != BodyMotion::eDynamic &&
bodyB->getMotionType() != BodyMotion::eDynamic )
continue;
Vec2f cpA = bodyA->mXForm.mul( c.posLocal[0] );
Vec2f cpB = bodyB->mXForm.mul( c.posLocal[1] );
//TODO: normal change need concerned
Vec2f normal = c.normal;
float depth = normal.dot( cpA - cpB ) + 0.001;
if ( depth <= 0 )
continue;
Vec2f cp = 0.5 * ( cpA + cpB );
Vec2f rA = cp - bodyA->mPosCenter;
Vec2f rB = cp - bodyB->mPosCenter;
float nrA = rA.cross( normal );
float nrB = rB.cross( normal );
float invMass = 0;
invMass += bodyA->mInvMass + bodyA->mInvI * nrA * nrA;
invMass += bodyB->mInvMass + bodyB->mInvI * nrB * nrB;
float offDepth = Math::Clamp ( ( depth - kSlopValue ) , 0 , kMaxDepth );
float impulse = ( invMass > 0 ) ? kValueB * offDepth / invMass : 0;
if ( impulse > 0 )
{
bodyA->mXForm.translate( -impulse * normal * bodyA->mInvMass );
bodyA->mRotationAngle += -impulse * nrA * bodyA->mInvI;
bodyA->synTransform();
bodyB->mXForm.translate( impulse * normal * bodyB->mInvMass );
bodyB->mRotationAngle += impulse * nrB * bodyB->mInvI;
bodyB->synTransform();
}
{
Vec2f cpA = bodyA->mXForm.mul( c.posLocal[0] );
Vec2f cpB = bodyB->mXForm.mul( c.posLocal[1] );
//TODO: normal change need concerned
float depth2 = normal.dot( cpA - cpB );
float dp = depth - depth2;
::Msg( "dp = %f " , dp );
int i = 1;
}
}
if ( maxDepth < 3 * kMaxDepth )
break;
}
if ( numMainfold != 0 )
jumpDebug();
}
void StageSelect::drawCenterBox(Vec2f pos, Vec2f size, Color color) {
drawFillBox(pos.x(), pos.y(), size.x(), size.y(), color, 0, Vec2f(1, 1), size / 2);
}
void MapScreen::scroll(Vec2f new_cam_pos, Camera& cam)
{
new_cam_pos = new_cam_pos + Vec2f(-16 - 8, -16 - 8);
cam.position(Vec2f(round(new_cam_pos.x()), round(new_cam_pos.y())));
}
void StageSelect::drawCenterTexture(Vec2f pos, Vec2f size, Texture image) {
drawTextureBox(pos.x(), pos.y(), size.x(), size.y(), 0, 0, image.width(), image.height(), image, Color::white,
0, Vec2f(1, 1), size / 2);
}
Vec2f GravityCalculation(Vec2f move)
{
return Vec2f(move.x(), move.y() - GR);
}
// Handle obstacles in a generic way by post-adjusting a set of commanded game vars
bool WAKGameShared::obstacleBallHandling(GameVars& GV) const
{
// Plot that obstacle ball handling is not active for the case that this function returns early
if(config.plotData())
PM.plotScalar(false * PMSCALE_LEGAL, PM_OBH_ACTIVE);
// Don't do anything if obstacle avoidance is disabled or we don't have a ball
if(!config.sEnableObstacles() || !config.obhEnableObstBallHandling() || !SV.haveBall)
return false;
// Don't do anything if the closest obstacle is not valid
if(!SV.obstClosest.valid())
return false;
// Calculate the ball to target unit vector
Vec2f ballToTargetVec = GV.ballTargetDir - SV.ballDir;
float ballToTargetDist = ballToTargetVec.norm();
if(ballToTargetDist <= 0.0f)
return false;
Vec2f txhat = ballToTargetVec / ballToTargetDist;
// Calculate the ball to obstacle unit vectors
Vec2f ballToObstVec = SV.obstClosest.vec - SV.ballDir;
float ballToObstDist = ballToObstVec.norm();
if(ballToObstDist <= 0.0f || ballToObstDist >= std::min(config.kickMaxDist(), ballToTargetDist + config.obhObstBeyondTargetBuf()))
return false;
Vec2f oxhat = ballToObstVec / ballToObstDist;
Vec2f oyhat = eigenRotatedCCW90(oxhat);
// Calculate the angle at the ball from the obstacle to the ball target
float angleAtBall = atan2(txhat.dot(oyhat), txhat.dot(oxhat));
bool ballBehindObst = (fabs(angleAtBall) > M_PI_2);
if(ballBehindObst && ballToObstDist > config.obhObstBeforeBallBuf())
return false;
// Wrap the angle at the ball to (-pi/2, pi/2] by factors of pi and adjust for the sign
float angleAtBallStd = angleAtBall;
if(angleAtBallStd > M_PI_2)
angleAtBallStd -= M_PI;
if(angleAtBallStd <= -M_PI_2)
angleAtBallStd += M_PI;
int angleAtBallStdSign = sign(angleAtBallStd);
angleAtBallStd = fabs(angleAtBallStd);
// Calculate the high and low angles at the ball, and the appropriate difference
float angleAtBallHigh = coerce<float>(asin(coerceAbs(config.obhObstClearanceHigh() / ballToObstDist, 1.0f)), 0.0f, config.obhClearanceAngleHighMax());
float angleAtBallLow = coerce<float>(asin(coerceAbs(config.obhObstClearanceLow() / ballToObstDist, 1.0f)), 0.0f, std::min(angleAtBallHigh, config.obhClearanceAngleLowMax()));
// Calculate the angle to adjust the target angle by
float angleAtBallDiff = angleAtBallHigh - angleAtBallLow;
float targetAngleAdjust = 0.0f;
if(angleAtBallStd < angleAtBallLow)
targetAngleAdjust = angleAtBallStdSign * interpolateCoerced(0.0f, angleAtBallLow, 0.0f, angleAtBallDiff, angleAtBallStd);
else if(angleAtBallStd < angleAtBallHigh)
targetAngleAdjust = angleAtBallStdSign * interpolateCoerced(angleAtBallLow, angleAtBallHigh, angleAtBallDiff, 0.0f, angleAtBallStd);
float targetAngleAdjustAbs = fabs(targetAngleAdjust);
// Calculate the angle adjust limits for dribbling and foot selection
float angleAdjustForDribble = coerceMin(0.5f * GV.ballTargetWedge * config.obhAngleAdjustWedgeRatio(), 0.0f);
float angleAdjustForFootSel = config.obhAngleAdjustForFootSel();
// See whether the obstacle is blocking
bool obstMightBlock = (angleAtBallDiff > angleAdjustForDribble);
bool obstIsBlocking = (obstMightBlock && (angleAtBallStd < angleAtBallLow || targetAngleAdjustAbs > angleAdjustForDribble));
// Disable kick and suggest a foot to use if the obstacle is blocking
bool kickIfPossible = !obstIsBlocking;
GameVars::FootSelection suggestFoot = GameVars::FS_EITHER_FOOT;
if(obstIsBlocking && ballToObstDist < config.obhFootSelObstBallDistMax())
{
if(targetAngleAdjust > angleAdjustForFootSel)
suggestFoot = (ballBehindObst ? GameVars::FS_LEFT_FOOT : GameVars::FS_RIGHT_FOOT);
else if(targetAngleAdjust < -angleAdjustForFootSel)
suggestFoot = (ballBehindObst ? GameVars::FS_RIGHT_FOOT : GameVars::FS_LEFT_FOOT);
}
// Decide whether the obstacle ball handling has to make an adjustment to the game variables
bool adjustNotNeeded = (targetAngleAdjustAbs <= 0.0f && kickIfPossible && suggestFoot == GameVars::FS_EITHER_FOOT);
// Plotting
if(config.plotData())
{
PM.plotScalar(!adjustNotNeeded * PMSCALE_LEGAL, PM_OBH_ACTIVE);
PM.plotScalar(ballToObstDist, PM_OBH_BALLTOOBSTDIST);
PM.plotScalar(angleAtBallLow, PM_OBH_ANGLEATBALLLOW);
PM.plotScalar(angleAtBallHigh, PM_OBH_ANGLEATBALLHIGH);
PM.plotScalar(angleAtBallStd, PM_OBH_ANGLEATBALLSTD);
PM.plotScalar(targetAngleAdjust, PM_OBH_TARGETANGLEADJUST);
PM.plotScalar(angleAdjustForDribble, PM_OBH_ANGLEADJUSTFORDRIBBLE);
PM.plotScalar(angleAdjustForFootSel, PM_OBH_ANGLEADJUSTFORFOOTSEL);
PM.plotScalar(kickIfPossible * PMSCALE_KICK, PM_OBH_KICKIFPOSSIBLE);
PM.plotScalar(suggestFoot, PM_OBH_SUGGESTFOOT);
}
// If no adjustment is required then we are fine
if(adjustNotNeeded)
return false;
// Calculate the new ball target as a rotation of the nominal ball target
Vec2f ballToTargetVecDes = eigenRotatedCCW(ballToTargetVec, targetAngleAdjust);
Vec2f ballTargetVec = SV.ballDir + ballToTargetVecDes;
// Update the game variables
GV.ballTargetDir = ballTargetVec;
if(GV.kickIfPossible)
GV.kickIfPossible = kickIfPossible;
if(GV.suggestFoot == GameVars::FS_EITHER_FOOT)
GV.suggestFoot = suggestFoot;
// Return that the game variables were modified
return true;
}
OGLText::OGLText(Vec2f position, const Color& color, const std::string& text, const char* fontLoc, float size) :_position(position.X(), position.Y(), -1.f), m_text(text), _fontLoc(fontLoc), _color(color)
{
m_font = new GLFont();
m_font->Create(_fontLoc);
//Create an approximation of how normal fontsizes work (Not 1:1, but close)
_size = size * (1 - (1 / m_font->m_height));
_width = _size * text.size();
_rot = 0.f;
}
int Flocking::update()
{
int i;
Vec2f centroid(0,0);
for(i = 0; i < boids.size(); i++)
{
centroid+=boids[i].loc;
if(boids[i].isHit(destination.x,destination.y,destinationArea))
{
boids[i].reachedDestination = true;
}
if(useCollisionFromSDF)
{
Vec2f dir = partialDerivaties[(int)boids[i].loc.x][(int)boids[i].loc.y];
float val = collisionSDF[(int)boids[i].loc.x][(int)boids[i].loc.y];
if(val==0)
val=0.001;
boids[i].seek(dir+boids[i].loc,dir.length()*collisionWeight/val);
}
if(sceneMap->getCell(boids[i].loc.x,boids[i].loc.y))
{
/*
vector<Vec2f> path;
path = pathFinder.getPath(sceneMap,boids[i].loc);
if(path.size()>1)
{
Vec2f dest = path[min((int)path.size()-1,10)]; //!@#
boids[i].seek(dest,destWeight); //seek the Goal !@#
}
*/
}
else
{
boids[i].hitObstacle = true;
}
//boids[i].seek(destinationSeek,destWeight); //seek the Goal !@#
boids[i].seek(destination,destWeight); //seek the Goal !@#
boids[i].update(boids);
if(boids[i].reachedDestination)
removeBoid(boids[i].loc.x,boids[i].loc.y,1); //ineffcient way to remove boids
}
centroid /=flockSize();
/*
if(sceneMap->getCell(centroid.x,centroid.y))
{
vector<Vec2f> path;
path = pathFinder.getPath(sceneMap,centroid);
if(path.size()>1)
{
destinationSeek = path[min((int)path.size()-1,5)];
}
}
*/
if(flockSize()==0)
return 0;
else
return 1;
}
void MyAppli::onExec () {
if (getClock("RequestTime").getElapsedTime().asSeconds() >= timeBtwnTwoReq.asSeconds()) {
std::string request = "GETCARPOS";
sf::Packet packet;
packet<<request;
Network::sendUdpPacket(packet);
getClock("RequestTime").restart();
received = false;
}
std::string response;
if (Network::getResponse("STOPCARMOVE", response)) {
std::vector<std::string> infos = split(response, "*");
int id = conversionStringInt(infos[0]);
Vec3f newPos (conversionStringFloat(infos[1]), conversionStringFloat(infos[2]), 0);
Caracter* caracter = static_cast<Caracter*>(World::getEntity(id));
Vec3f actualPos = Vec3f(caracter->getCenter().x, caracter->getCenter().y, 0);
if (hero->getId() == id) {
for (unsigned int i = 0; i < getRenderComponentManager().getNbComponents(); i++) {
View view = getRenderComponentManager().getRenderComponent(i)->getView();
Vec3f d = newPos - view.getPosition();
view.move(d.x, d.y, d.y);
getRenderComponentManager().getRenderComponent(i)->setView(view);
}
Vec3f d = newPos - getView().getPosition();
getView().move(d.x, d.y, d.y);
}
Vec3f d = newPos - actualPos;
World::moveEntity(caracter, d.x, d.y, d.y);
caracter->setMoving(false);
World::update();
}
if (Network::getResponse("MONSTERONMOUSE", response)) {
std::cout<<"monster on mouse!"<<std::endl;
}
if (Network::getResponse("NEWPATH", response)) {
std::vector<std::string> infos = split(response, "*");
std::vector<Vec2f> path;
int size = conversionStringInt(infos[0]);
int id = conversionStringInt(infos[1]);
Caracter* caracter = static_cast<Caracter*>(World::getEntity(id));
Vec2f actualPos (conversionStringFloat(infos[2]), conversionStringFloat(infos[3]));
Vec2f newPos = Computer::getPosOnPathFromTime(actualPos, caracter->getPath(),ping,caracter->getSpeed());
for (int i = 0; i < size; i++) {
path.push_back(Vec2f(conversionStringFloat(infos[i*2+4]), conversionStringFloat(infos[i*2+5])));
}
Vec2f d = newPos - actualPos;
Vec2f dir = d.normalize();
if (dir != caracter->getDir())
caracter->setDir(dir);
World::moveEntity(caracter, d.x, d.y, d.y);
caracter->setPath(path);
caracter->setMoving(true);
caracter->interpolation.first = caracter->getCenter();
caracter->interpolation.second = Computer::getPosOnPathFromTime(caracter->interpolation.first, caracter->getPath(),ping + timeBtwnTwoReq.asMicroseconds(),caracter->getSpeed());
caracter->getClkTransfertTime().restart();
}
if (Network::getResponse("NEWPOS", response)) {
std::vector<std::string> infos = split(response, "*");
if (infos.size() == 4) {
int id = conversionStringInt(infos[0]);
ping = conversionStringLong(infos[1]);
Caracter* caracter = static_cast<Caracter*>(World::getEntity(id));
Vec3f actualPos = Vec3f(caracter->getCenter().x, caracter->getCenter().y, 0);
Vec3f newPos (conversionStringFloat(infos[2]), conversionStringFloat(infos[3]), 0);
Vec3f d = newPos - actualPos;
if (id == hero->getId()) {
for (unsigned int i = 0; i < getRenderComponentManager().getNbComponents(); i++) {
View view = getRenderComponentManager().getRenderComponent(i)->getView();
view.move(d.x, d.y, d.y);
getRenderComponentManager().getRenderComponent(i)->setView(view);
}
getView().move (d.x, d.y, d.y);
}
World::moveEntity(caracter, d.x, d.y, d.y);
World::update();
caracter->interpolation.first = Vec3f(caracter->getCenter().x, caracter->getCenter().y, 0);
if (caracter->isMoving()) {
if (caracter->isMovingFromKeyboard()) {
caracter->interpolation.second = caracter->interpolation.first + Vec3f(caracter->getDir().x,caracter->getDir().y,0) * caracter->getSpeed() * (ping + timeBtwnTwoReq.asMicroseconds());
} else {
caracter->interpolation.second = Computer::getPosOnPathFromTime(caracter->interpolation.first, caracter->getPath(),ping + timeBtwnTwoReq.asMicroseconds(),caracter->getSpeed());
}
} else {
caracter->interpolation.second = caracter->interpolation.first;
}
caracter->getClkTransfertTime().restart();
}
} else {
std::vector<Entity*> caracters = World::getEntities("E_MONSTER+E_HERO");
for (unsigned int i = 0; i < caracters.size(); i++) {
Caracter* caracter = static_cast<Caracter*>(caracters[i]);
if (caracter->isMoving()) {
if (caracter->isMovingFromKeyboard()) {
Vec3f actualPos = Vec3f(caracter->getCenter().x, caracter->getCenter().y, 0);
sf::Int64 elapsedTime = caracter->getClkTransfertTime().getElapsedTime().asMicroseconds();
Vec3f newPos = caracter->interpolation.first + (caracter->interpolation.second - caracter->interpolation.first) * ((float) elapsedTime / (float) (ping + timeBtwnTwoReq.asMicroseconds()));
Ray ray(actualPos, newPos);
if (World::collide(caracter, ray)) {
newPos = actualPos;
}
for (unsigned int i = 0; i < getRenderComponentManager().getNbComponents(); i++) {
View view = getRenderComponentManager().getRenderComponent(i)->getView();
Vec3f d = newPos - view.getPosition();
view.move(d.x, d.y, d.y);
getRenderComponentManager().getRenderComponent(i)->setView(view);
}
Vec3f d = newPos - actualPos;
World::moveEntity(caracter, d.x, d.y, d.y);
getView().move(d.x, d.y, d.y);
World::update();
} else {
Vec3f actualPos (caracter->getCenter().x, caracter->getCenter().y, 0);
sf::Int64 elapsedTime = caracter->getClkTransfertTime().getElapsedTime().asMicroseconds();
Vec3f newPos = caracter->interpolation.first + (caracter->interpolation.second - caracter->interpolation.first) * ((float) elapsedTime / (float) (ping + timeBtwnTwoReq.asMicroseconds()));
Vec3f d = newPos - actualPos;
if (newPos.computeDist(caracter->getPath()[caracter->getPath().size() - 1]) <= PATH_ERROR_MARGIN) {
caracter->setMoving(false);
newPos = caracter->getPath()[caracter->getPath().size() - 1];
}
if (caracter->getId() == hero->getId()) {
for (unsigned int i = 0; i < getRenderComponentManager().getNbComponents(); i++) {
View view = getRenderComponentManager().getRenderComponent(i)->getView();
view.move(d.x, d.y, d.y);
getRenderComponentManager().getRenderComponent(i)->setView(view);
}
getView().move(d.x, d.y, d.y);
}
Vec2f dir = d.normalize();
if (dir != caracter->getDir())
caracter->setDir(dir);
World::moveEntity(caracter, d.x, d.y, d.y);
World::update();
}
}
}
}
if (hero->isInFightingMode()) {
if (hero->getFocusedCaracter() != nullptr) {
int distToEnnemi = hero->getCenter().computeDist(hero->getFocusedCaracter()->getCenter());
if (distToEnnemi <= hero->getRange()) {
if (hero->isMoving())
hero->setMoving(false);
hero->setAttacking(true);
hero->attackFocusedCaracter();
} else {
hero->setAttacking(false);
}
}
}
}
void Button::drawInternal(Graphics* const TheGraphics, Real32 Opacity) const
{
Pnt2f TopLeft, BottomRight;
getInsideBorderBounds(TopLeft, BottomRight);
//If I have a DrawObject then Draw it
UIDrawObjectCanvasRefPtr DrawnDrawObject = getDrawnDrawObject();
UIDrawObjectCanvasRefPtr BaseDrawObject = getBaseDrawObject();
if(DrawnDrawObject != NULL)
{
//Get the Draw Object Size
Pnt2f DrawObjectTopLeft, DrawObjectBottomRight;
DrawnDrawObject->getDrawObjectBounds(DrawObjectTopLeft, DrawObjectBottomRight);
Pnt2f BaseDrawObjectTopLeft, BaseDrawObjectBottomRight;
BaseDrawObject->getDrawObjectBounds(BaseDrawObjectTopLeft, BaseDrawObjectBottomRight);
if(getText() != "" && getFont() != NULL)
{
//Draw both the text and Draw Object
//Get the Text Size
Pnt2f TextTopLeft, TextBottomRight;
getFont()->getBounds(getText(), TextTopLeft, TextBottomRight);
Vec2f InternalsSize;
if(getDrawObjectToTextAlignment() == ALIGN_DRAW_OBJECT_LEFT_OF_TEXT ||
getDrawObjectToTextAlignment() == ALIGN_DRAW_OBJECT_RIGHT_OF_TEXT)
{
InternalsSize.setValues((TextBottomRight.x()-TextTopLeft.x()) + (DrawObjectBottomRight.x()-DrawObjectTopLeft.x()) + getDrawObjectToTextPadding(),
osgMax((TextBottomRight.y()-TextTopLeft.y()), (DrawObjectBottomRight.y()-DrawObjectTopLeft.y())));
}
else
{
InternalsSize.setValues(osgMax((TextBottomRight.x()-TextTopLeft.x()), (DrawObjectBottomRight.x()-DrawObjectTopLeft.x())),
(TextBottomRight.y()-TextTopLeft.y()) + (DrawObjectBottomRight.y()-DrawObjectTopLeft.y()) + getDrawObjectToTextPadding());
}
Pnt2f InternalAlignment;
InternalAlignment = calculateAlignment(TopLeft, (BottomRight-TopLeft), InternalsSize,getAlignment().y(), getAlignment().x());
//Draw Object Alignment
Pnt2f DrawObjectAlignedPosition;
switch(getDrawObjectToTextAlignment())
{
case ALIGN_DRAW_OBJECT_LEFT_OF_TEXT:
DrawObjectAlignedPosition = calculateAlignment(InternalAlignment, InternalsSize, (BaseDrawObjectBottomRight - BaseDrawObjectTopLeft),0.5f, 0.0);
break;
case ALIGN_DRAW_OBJECT_RIGHT_OF_TEXT:
DrawObjectAlignedPosition = calculateAlignment(InternalAlignment, InternalsSize, (BaseDrawObjectBottomRight - BaseDrawObjectTopLeft),0.5f, 1.0);
break;
case ALIGN_DRAW_OBJECT_ABOVE_TEXT:
DrawObjectAlignedPosition = calculateAlignment(InternalAlignment, InternalsSize, (BaseDrawObjectBottomRight - BaseDrawObjectTopLeft),0.0f, 0.5);
break;
case ALIGN_DRAW_OBJECT_BELOW_TEXT:
DrawObjectAlignedPosition = calculateAlignment(InternalAlignment, InternalsSize, (BaseDrawObjectBottomRight - BaseDrawObjectTopLeft),1.0f, 0.5);
break;
}
//If active then translate the Text by the Active Offset
DrawObjectAlignedPosition = DrawObjectAlignedPosition + getDrawnOffset();
DrawnDrawObject->setPosition( DrawObjectAlignedPosition );
//Text Alignment
Pnt2f TextAlignedPosition;
switch(getDrawObjectToTextAlignment())
{
case ALIGN_DRAW_OBJECT_LEFT_OF_TEXT:
TextAlignedPosition = calculateAlignment(InternalAlignment, InternalsSize, (TextBottomRight - TextTopLeft),0.5f, 1.0);
break;
case ALIGN_DRAW_OBJECT_RIGHT_OF_TEXT:
TextAlignedPosition = calculateAlignment(InternalAlignment, InternalsSize, (TextBottomRight - TextTopLeft),0.5f, 0.0);
break;
case ALIGN_DRAW_OBJECT_ABOVE_TEXT:
TextAlignedPosition = calculateAlignment(InternalAlignment, InternalsSize, (TextBottomRight - TextTopLeft),1.0f, 0.5);
break;
case ALIGN_DRAW_OBJECT_BELOW_TEXT:
TextAlignedPosition = calculateAlignment(InternalAlignment, InternalsSize, (TextBottomRight - TextTopLeft),0.0f, 0.5);
break;
}
drawText(TheGraphics, TextAlignedPosition, Opacity);
}
else
{
//Just Draw the Draw Object
Pnt2f AlignedPosition;
AlignedPosition = calculateAlignment(TopLeft, (BottomRight-TopLeft), (BaseDrawObjectBottomRight - BaseDrawObjectTopLeft),getAlignment().y(), getAlignment().x());
//If active then translate the Text by the Active Offset
AlignedPosition = AlignedPosition + getDrawnOffset();
DrawnDrawObject->setPosition( AlignedPosition );
}
//Draw the DrawnDrawObject
DrawnDrawObject->draw(TheGraphics, getOpacity()*Opacity);
}
else if(getText() != "" && getFont() != NULL)
{
//Just Draw the Text
Pnt2f AlignedPosition;
Pnt2f TextTopLeft, TextBottomRight;
getFont()->getBounds(getText(), TextTopLeft, TextBottomRight);
AlignedPosition = calculateAlignment(TopLeft, (BottomRight-TopLeft), (TextBottomRight - TextTopLeft),getAlignment().y(), getAlignment().x());
drawText(TheGraphics, AlignedPosition, Opacity);
}
}
//@todo: убрать\прокомментить этот феерический высер
void TargetEnergySystem::update(EntityPtr entity)
{
if(!HasCmpt(TargetComponent, entity))
return;
GetCmpt(TargetComponent, target_com, entity);
//If target is null, dissappear
if(!target_com->target.is_set())
{
entity->mark_deleted();
return;
}
EntityPtr target = target_com->target;
GetCmpt(EnergyStorageComponent, enesto, target);
GetCmpt(PositionComponent, target_pos_com, target);
GetCmpt(NodeComponent, target_node_com, target);
GetCmpt(MovementComponent, move_com, entity);
GetCmpt(PositionComponent, pos_com, entity);
//If energy reached target
Vec2f dist = target_pos_com->position - pos_com->position;
bool target_changed = false;
if(dist.length() < 0.01f)
{
//Check, is target enemy tower
if(target_com->target_enemy)
{
GetCmpt(EnergyStorageComponent, target_enesto_cmpt, target_com->target);
target_enesto_cmpt->rem_energy(5);
entity->mark_deleted();
return;
}
//@todo: changing entity position thoughtlessly is VERY dangerous!
// it cost 3 evenings of debug.
// Reposition here break GameLogic entity iterator
// Changing position without changing cell can produce entityPtr
// duplicate on two cells.
// @todo: changing entity position should be on map or gamelogic.
/*pos_com->old_position = pos_com->position;
pos_com->position = target_pos_com->position;
GameEngine::get_data()->logic.getMap()->RepositionEntityToCorrectCell
(entity, pos_com->old_position, pos_com->position);*/
EntityIt it = target_node_com->children.begin();
EntityPtr next_target = target;
float min_energy_balance = 1.0f;
while (it != target_node_com->children.end())
{
GetCmpt(EnergyStorageComponent, enesto_child, (it->get()));
if(enesto_child->balance < min_energy_balance)
{
next_target = (*it);
min_energy_balance = enesto_child->balance;
}
it++;
}
if(min_energy_balance == 1.0) //children are full (or not exist)
{
if(!enesto->is_full())
merge_energy(target, entity);
else
{
// All childs are full and we have reached base tower.
if(!target_node_com->parent.is_set())
{
entity->mark_deleted();
return;
}
target_com->target = target_node_com->parent;
target_changed = true;
}
}
else
{
target_com->target = next_target;
target_changed = true;
}
}
if(target_changed)
{
GetCmpt(PositionComponent, new_target_pos, target_com->target);
dist = new_target_pos->position - pos_com->position;
}
// Energy, moving to enemy target, move unlinear
if (target_com->target_enemy)
{
}
else
{
move_com->speed = dist;
move_com->speed.length(0.5f); //0.5 per sec.
}
}
Vec2f Pendulum::AnGravity(Vec2f pos_, Vec2f sPos_, float rotate_, float rotateS_, float length_)
{
Vec2f putPos = pos_;
// 現在の角度
float rad = rotate_ * M_PI / 180;
// 現在の重りの位置
putPos.x() = sPos_.x() + cos(rad) * length_;
putPos.y() = sPos_.y() + sin(rad) * length_;
// 重りと支点との距離を算出
float vx = putPos.x() - sPos_.x();
float vy = putPos.y() - sPos_.y();
//？
float t = -(vy * 0) / (vx * vx + vy * vy);
// 重力移動量を反映した重りの位置
float gx = putPos.x() + t * vx;
float gy = putPos.y() + t * vy;
// ２つの重りの位置の角度差
float r = atan2(gy - sPos_.y(), gx - sPos_.x()) * 180 / M_PI;
//現在の角度二つの重りの角度差との差を算出
float sub = r - rotate_;
//角度の誤差をfloorで修正
sub -= floor(sub / 360.0) * 360.0;
// 角度差を角速度に加算
if (sub <-180.0) sub += 360.0;
if (sub > 180.0) sub -= 360.0;
rotateS_ += sub;
// 摩擦
// rot_spd *= 0.995;
// 角度に角速度を加算
rotate_ += rotateS_;
// 新しい重りの位置
rad = rotate_ * M_PI / 180;
putPos.x() = sPos_.x() + cos(rad) * length_;
putPos.y() = sPos_.y() + sin(rad) * length_;
// 重りの座標
return putPos;
}
// React to size changes
void reshape(Vec2f Size)
{
mgr->resize(Size.x(), Size.y());
}
int main() {
const std::string sInputDir = stlplus::folder_up(string(THIS_SOURCE_DIR))
+ "/imageData/SceauxCastle/";
const string jpg_filenameL = sInputDir + "100_7101.jpg";
const string jpg_filenameR = sInputDir + "100_7102.jpg";
Image<unsigned char> imageL, imageR;
ReadImage(jpg_filenameL.c_str(), &imageL);
ReadImage(jpg_filenameR.c_str(), &imageR);
//--
// Detect regions thanks to an image_describer
//--
using namespace openMVG::features;
std::unique_ptr<Image_describer> image_describer(new SIFT_Image_describer);
std::map<IndexT, std::unique_ptr<features::Regions> > regions_perImage;
image_describer->Describe(imageL, regions_perImage[0]);
image_describer->Describe(imageR, regions_perImage[1]);
const SIFT_Regions* regionsL = dynamic_cast<SIFT_Regions*>(regions_perImage.at(0).get());
const SIFT_Regions* regionsR = dynamic_cast<SIFT_Regions*>(regions_perImage.at(1).get());
const PointFeatures
featsL = regions_perImage.at(0)->GetRegionsPositions(),
featsR = regions_perImage.at(1)->GetRegionsPositions();
// Show both images side by side
{
Image<unsigned char> concat;
ConcatH(imageL, imageR, concat);
string out_filename = "01_concat.jpg";
WriteImage(out_filename.c_str(), concat);
}
//- Draw features on the two image (side by side)
{
Image<unsigned char> concat;
ConcatH(imageL, imageR, concat);
//-- Draw features :
for (size_t i=0; i < featsL.size(); ++i ) {
const SIOPointFeature point = regionsL->Features()[i];
DrawCircle(point.x(), point.y(), point.scale(), 255, &concat);
}
for (size_t i=0; i < featsR.size(); ++i ) {
const SIOPointFeature point = regionsR->Features()[i];
DrawCircle(point.x()+imageL.Width(), point.y(), point.scale(), 255, &concat);
}
string out_filename = "02_features.jpg";
WriteImage(out_filename.c_str(), concat);
}
std::vector<IndMatch> vec_PutativeMatches;
//-- Perform matching -> find Nearest neighbor, filtered with Distance ratio
{
// Define a matcher and a metric to find corresponding points
typedef SIFT_Regions::DescriptorT DescriptorT;
typedef L2_Vectorized<DescriptorT::bin_type> Metric;
typedef ArrayMatcherBruteForce<DescriptorT::bin_type, Metric> MatcherT;
// Distance ratio squared due to squared metric
getPutativesMatches<DescriptorT, MatcherT>(
((SIFT_Regions*)regions_perImage.at(0).get())->Descriptors(),
((SIFT_Regions*)regions_perImage.at(1).get())->Descriptors(),
Square(0.8), vec_PutativeMatches);
IndMatchDecorator<float> matchDeduplicator(
vec_PutativeMatches, featsL, featsR);
matchDeduplicator.getDeduplicated(vec_PutativeMatches);
std::cout
<< regions_perImage.at(0)->RegionCount() << " #Features on image A" << std::endl
<< regions_perImage.at(1)->RegionCount() << " #Features on image B" << std::endl
<< vec_PutativeMatches.size() << " #matches with Distance Ratio filter" << std::endl;
// Draw correspondences after Nearest Neighbor ratio filter
svgDrawer svgStream( imageL.Width() + imageR.Width(), max(imageL.Height(), imageR.Height()));
svgStream.drawImage(jpg_filenameL, imageL.Width(), imageL.Height());
svgStream.drawImage(jpg_filenameR, imageR.Width(), imageR.Height(), imageL.Width());
for (size_t i = 0; i < vec_PutativeMatches.size(); ++i) {
//Get back linked feature, draw a circle and link them by a line
const SIOPointFeature L = regionsL->Features()[vec_PutativeMatches[i]._i];
const SIOPointFeature R = regionsR->Features()[vec_PutativeMatches[i]._j];
svgStream.drawLine(L.x(), L.y(), R.x()+imageL.Width(), R.y(), svgStyle().stroke("green", 2.0));
svgStream.drawCircle(L.x(), L.y(), L.scale(), svgStyle().stroke("yellow", 2.0));
svgStream.drawCircle(R.x()+imageL.Width(), R.y(), R.scale(),svgStyle().stroke("yellow", 2.0));
}
const std::string out_filename = "03_siftMatches.svg";
std::ofstream svgFile( out_filename.c_str() );
svgFile << svgStream.closeSvgFile().str();
svgFile.close();
}
// Essential geometry filtering of putative matches
{
Mat3 K;
//read K from file
if (!readIntrinsic(stlplus::create_filespec(sInputDir,"K","txt"), K))
{
std::cerr << "Cannot read intrinsic parameters." << std::endl;
return EXIT_FAILURE;
}
//A. prepare the corresponding putatives points
Mat xL(2, vec_PutativeMatches.size());
Mat xR(2, vec_PutativeMatches.size());
for (size_t k = 0; k < vec_PutativeMatches.size(); ++k) {
const PointFeature & imaL = featsL[vec_PutativeMatches[k]._i];
const PointFeature & imaR = featsR[vec_PutativeMatches[k]._j];
xL.col(k) = imaL.coords().cast<double>();
xR.col(k) = imaR.coords().cast<double>();
}
//B. Compute the relative pose thanks to a essential matrix estimation
std::pair<size_t, size_t> size_imaL(imageL.Width(), imageL.Height());
std::pair<size_t, size_t> size_imaR(imageR.Width(), imageR.Height());
sfm::RelativePose_Info relativePose_info;
if (!sfm::robustRelativePose(K, K, xL, xR, relativePose_info, size_imaL, size_imaR, 256))
{
std::cerr << " /!\\ Robust relative pose estimation failure."
<< std::endl;
return EXIT_FAILURE;
}
std::cout << "\nFound an Essential matrix:\n"
<< "\tprecision: " << relativePose_info.found_residual_precision << " pixels\n"
<< "\t#inliers: " << relativePose_info.vec_inliers.size() << "\n"
<< "\t#matches: " << vec_PutativeMatches.size()
<< std::endl;
// Show Essential validated point
svgDrawer svgStream( imageL.Width() + imageR.Width(), max(imageL.Height(), imageR.Height()));
svgStream.drawImage(jpg_filenameL, imageL.Width(), imageL.Height());
svgStream.drawImage(jpg_filenameR, imageR.Width(), imageR.Height(), imageL.Width());
for (size_t i = 0; i < relativePose_info.vec_inliers.size(); ++i) {
const SIOPointFeature & LL = regionsL->Features()[vec_PutativeMatches[relativePose_info.vec_inliers[i]]._i];
const SIOPointFeature & RR = regionsR->Features()[vec_PutativeMatches[relativePose_info.vec_inliers[i]]._j];
const Vec2f L = LL.coords();
const Vec2f R = RR.coords();
svgStream.drawLine(L.x(), L.y(), R.x()+imageL.Width(), R.y(), svgStyle().stroke("green", 2.0));
svgStream.drawCircle(L.x(), L.y(), LL.scale(), svgStyle().stroke("yellow", 2.0));
svgStream.drawCircle(R.x()+imageL.Width(), R.y(), RR.scale(),svgStyle().stroke("yellow", 2.0));
}
const std::string out_filename = "04_ACRansacEssential.svg";
std::ofstream svgFile( out_filename.c_str() );
svgFile << svgStream.closeSvgFile().str();
svgFile.close();
//C. Triangulate and export inliers as a PLY scene
std::vector<Vec3> vec_3DPoints;
// Setup camera intrinsic and poses
Pinhole_Intrinsic intrinsic0(imageL.Width(), imageL.Height(), K(0, 0), K(0, 2), K(1, 2));
Pinhole_Intrinsic intrinsic1(imageR.Width(), imageR.Height(), K(0, 0), K(0, 2), K(1, 2));
const Pose3 pose0 = Pose3(Mat3::Identity(), Vec3::Zero());
const Pose3 pose1 = relativePose_info.relativePose;
// Init structure by inlier triangulation
const Mat34 P1 = intrinsic0.get_projective_equivalent(pose0);
const Mat34 P2 = intrinsic1.get_projective_equivalent(pose1);
std::vector<double> vec_residuals;
vec_residuals.reserve(relativePose_info.vec_inliers.size() * 4);
for (size_t i = 0; i < relativePose_info.vec_inliers.size(); ++i) {
const SIOPointFeature & LL = regionsL->Features()[vec_PutativeMatches[relativePose_info.vec_inliers[i]]._i];
const SIOPointFeature & RR = regionsR->Features()[vec_PutativeMatches[relativePose_info.vec_inliers[i]]._j];
// Point triangulation
Vec3 X;
TriangulateDLT(P1, LL.coords().cast<double>(), P2, RR.coords().cast<double>(), &X);
// Reject point that is behind the camera
if (pose0.depth(X) < 0 && pose1.depth(X) < 0)
continue;
const Vec2 residual0 = intrinsic0.residual(pose0, X, LL.coords().cast<double>());
const Vec2 residual1 = intrinsic1.residual(pose1, X, RR.coords().cast<double>());
vec_residuals.push_back(fabs(residual0(0)));
vec_residuals.push_back(fabs(residual0(1)));
vec_residuals.push_back(fabs(residual1(0)));
vec_residuals.push_back(fabs(residual1(1)));
}
// Display some statistics of reprojection errors
float dMin, dMax, dMean, dMedian;
minMaxMeanMedian<float>(vec_residuals.begin(), vec_residuals.end(),
dMin, dMax, dMean, dMedian);
std::cout << std::endl
<< "Triangulation residuals statistics:" << "\n"
<< "\t-- Residual min:\t" << dMin << "\n"
<< "\t-- Residual median:\t" << dMedian << "\n"
<< "\t-- Residual max:\t " << dMax << "\n"
<< "\t-- Residual mean:\t " << dMean << std::endl;
// Export as PLY (camera pos + 3Dpoints)
std::vector<Vec3> vec_camPos;
vec_camPos.push_back( pose0.center() );
vec_camPos.push_back( pose1.center() );
exportToPly(vec_3DPoints, vec_camPos, "EssentialGeometry.ply");
}
return EXIT_SUCCESS;
}
