void Foam::CV2D::newPoints()
{
const scalar relaxation = relaxationModel_->relaxation();
Info<< "Relaxation = " << relaxation << endl;
Field<point2D> dualVertices(number_of_faces());
label dualVerti = 0;
// Find the dual point of each tetrahedron and assign it an index.
for
(
Triangulation::Finite_faces_iterator fit = finite_faces_begin();
fit != finite_faces_end();
++fit
)
{
fit->faceIndex() = -1;
if
(
fit->vertex(0)->internalOrBoundaryPoint()
|| fit->vertex(1)->internalOrBoundaryPoint()
|| fit->vertex(2)->internalOrBoundaryPoint()
)
{
fit->faceIndex() = dualVerti;
dualVertices[dualVerti] = toPoint2D(circumcenter(fit));
dualVerti++;
}
}
dualVertices.setSize(dualVerti);
Field<vector2D> displacementAccumulator
(
startOfSurfacePointPairs_,
vector2D::zero
);
// Calculate target size and alignment for vertices
scalarField sizes
(
number_of_vertices(),
meshControls().minCellSize()
);
Field<vector2D> alignments
(
number_of_vertices(),
vector2D(1, 0)
);
for
(
Triangulation::Finite_vertices_iterator vit = finite_vertices_begin();
vit != finite_vertices_end();
++vit
)
{
if (vit->internalOrBoundaryPoint())
{
point2D vert = toPoint2D(vit->point());
// alignment and size determination
pointIndexHit pHit;
label hitSurface = -1;
qSurf_.findSurfaceNearest
(
toPoint3D(vert),
meshControls().span2(),
pHit,
hitSurface
);
if (pHit.hit())
{
vectorField norm(1);
allGeometry_[hitSurface].getNormal
(
List<pointIndexHit>(1, pHit),
norm
);
alignments[vit->index()] = toPoint2D(norm[0]);
sizes[vit->index()] =
cellSizeControl_.cellSize
(
toPoint3D(vit->point())
);
}
}
}
// Info<< "Calculated alignments" << endl;
scalar cosAlignmentAcceptanceAngle = 0.68;
// Upper and lower edge length ratios for weight
scalar u = 1.0;
scalar l = 0.7;
PackedBoolList pointToBeRetained(startOfSurfacePointPairs_, true);
std::list<Point> pointsToInsert;
for
(
Triangulation::Finite_edges_iterator eit = finite_edges_begin();
eit != finite_edges_end();
eit++
)
{
Vertex_handle vA = eit->first->vertex(cw(eit->second));
Vertex_handle vB = eit->first->vertex(ccw(eit->second));
if (!vA->internalOrBoundaryPoint() || !vB->internalOrBoundaryPoint())
{
continue;
}
const point2D& dualV1 = dualVertices[eit->first->faceIndex()];
const point2D& dualV2 =
dualVertices[eit->first->neighbor(eit->second)->faceIndex()];
scalar dualEdgeLength = mag(dualV1 - dualV2);
point2D dVA = toPoint2D(vA->point());
point2D dVB = toPoint2D(vB->point());
Field<vector2D> alignmentDirsA(2);
alignmentDirsA[0] = alignments[vA->index()];
alignmentDirsA[1] = vector2D
(
-alignmentDirsA[0].y(),
alignmentDirsA[0].x()
);
Field<vector2D> alignmentDirsB(2);
alignmentDirsB[0] = alignments[vB->index()];
alignmentDirsB[1] = vector2D
(
-alignmentDirsB[0].y(),
alignmentDirsB[0].x()
);
Field<vector2D> alignmentDirs(alignmentDirsA);
forAll(alignmentDirsA, aA)
{
const vector2D& a(alignmentDirsA[aA]);
scalar maxDotProduct = 0.0;
forAll(alignmentDirsB, aB)
{
const vector2D& b(alignmentDirsB[aB]);
scalar dotProduct = a & b;
if (mag(dotProduct) > maxDotProduct)
{
maxDotProduct = mag(dotProduct);
alignmentDirs[aA] = a + sign(dotProduct)*b;
alignmentDirs[aA] /= mag(alignmentDirs[aA]);
}
}
}
vector2D rAB = dVA - dVB;
scalar rABMag = mag(rAB);
forAll(alignmentDirs, aD)
{
vector2D& alignmentDir = alignmentDirs[aD];
if ((rAB & alignmentDir) < 0)
{
// swap the direction of the alignment so that has the
// same sense as rAB
alignmentDir *= -1;
}
scalar alignmentDotProd = ((rAB/rABMag) & alignmentDir);
if (alignmentDotProd > cosAlignmentAcceptanceAngle)
{
scalar targetFaceSize =
0.5*(sizes[vA->index()] + sizes[vB->index()]);
// Test for changing aspect ratio on second alignment (first
// alignment is neartest surface normal)
// if (aD == 1)
// {
// targetFaceSize *= 2.0;
// }
alignmentDir *= 0.5*targetFaceSize;
vector2D delta = alignmentDir - 0.5*rAB;
if (dualEdgeLength < 0.7*targetFaceSize)
{
delta *= 0;
}
else if (dualEdgeLength < targetFaceSize)
{
delta *=
(
dualEdgeLength
/(targetFaceSize*(u - l))
- 1/((u/l) - 1)
);
}
if
(
vA->internalPoint()
&& vB->internalPoint()
&& rABMag > 1.75*targetFaceSize
&& dualEdgeLength > 0.05*targetFaceSize
&& alignmentDotProd > 0.93
)
{
// Point insertion
pointsToInsert.push_back(toPoint(0.5*(dVA + dVB)));
}
else if
(
(vA->internalPoint() || vB->internalPoint())
&& rABMag < 0.65*targetFaceSize
)
{
// Point removal
// Only insert a point at the midpoint of the short edge
// if neither attached point has already been identified
// to be removed.
if
(
pointToBeRetained[vA->index()] == true
&& pointToBeRetained[vB->index()] == true
)
{
pointsToInsert.push_back(toPoint(0.5*(dVA + dVB)));
}
if (vA->internalPoint())
{
pointToBeRetained[vA->index()] = false;
}
if (vB->internalPoint())
{
pointToBeRetained[vB->index()] = false;
}
}
else
{
if (vA->internalPoint())
{
displacementAccumulator[vA->index()] += delta;
}
if (vB->internalPoint())
{
displacementAccumulator[vB->index()] += -delta;
}
}
}
}
}
void Foam::CV2D::calcDual
(
point2DField& dualPoints,
faceList& dualFaces,
wordList& patchNames,
labelList& patchSizes,
EdgeMap<label>& mapEdgesRegion,
EdgeMap<label>& indirectPatchEdge
) const
{
// Dual points stored in triangle order.
dualPoints.setSize(number_of_faces());
label dualVerti = 0;
for
(
Triangulation::Finite_faces_iterator fit = finite_faces_begin();
fit != finite_faces_end();
++fit
)
{
if
(
fit->vertex(0)->internalOrBoundaryPoint()
|| fit->vertex(1)->internalOrBoundaryPoint()
|| fit->vertex(2)->internalOrBoundaryPoint()
)
{
fit->faceIndex() = dualVerti;
dualPoints[dualVerti++] = toPoint2D(circumcenter(fit));
}
else
{
fit->faceIndex() = -1;
}
}
dualPoints.setSize(dualVerti);
extractPatches(patchNames, patchSizes, mapEdgesRegion, indirectPatchEdge);
forAll(patchNames, patchi)
{
Info<< "Patch " << patchNames[patchi]
<< " has size " << patchSizes[patchi] << endl;
}
// Create dual faces
// ~~~~~~~~~~~~~~~~~
dualFaces.setSize(number_of_vertices());
label dualFacei = 0;
labelList faceVerts(maxNvert);
for
(
Triangulation::Finite_vertices_iterator vit = finite_vertices_begin();
vit != finite_vertices_end();
++vit
)
{
if (vit->internalOrBoundaryPoint())
{
Face_circulator fcStart = incident_faces(vit);
Face_circulator fc = fcStart;
label verti = 0;
do
{
if (!is_infinite(fc))
{
if (fc->faceIndex() < 0)
{
FatalErrorInFunction
<< "Dual face uses vertex defined by a triangle"
" defined by an external point"
<< exit(FatalError);
}
// Look up the index of the triangle
faceVerts[verti++] = fc->faceIndex();
}
} while (++fc != fcStart);
if (faceVerts.size() > 2)
{
dualFaces[dualFacei++] =
face(labelList::subList(faceVerts, verti));
}
else
{
Info<< "From triangle point:" << vit->index()
<< " coord:" << toPoint2D(vit->point())
<< " generated illegal dualFace:" << faceVerts
<< endl;
}
}
}
dualFaces.setSize(dualFacei);
}
void Gesture1::trackMarker (IplImage* destImg, CvPoint _r, CvPoint _b, CvPoint _g, CvPoint _y) {
// find tissue box!
CvPoint* objPoints = objectDetector->detect(destImg);
// draw
world->Step(1.0F/6.0F, 10, 10);
cvLine(destImg, cvPoint(0,HEIGHT), cvPoint(1000,HEIGHT), CV_RGB(0,255,0), 3);
for (b2Body* b = world->GetBodyList(); b; b = b->GetNext()) {
//printf("**draw body\n");
Box2DData* userData = (Box2DData*)b->GetUserData();
if (userData != NULL) {
if (strcmp(userData->type, "Circle") == 0) {
//b2Vec2 v = b->GetWorldCenter();
b2Vec2 v = b->GetPosition();
//printf("** x=%f y=%f r=%f\n", v.x, v.y, userData->radius);
CvPoint center = cvPoint(v.x*WORLD_SCALE, v.y*WORLD_SCALE);
cvCircle(destImg, center, userData->radius*WORLD_SCALE, CV_RGB(255,0,0), -1);
} else if (strcmp(userData->type, "Box") == 0) {
world->DestroyBody(b);
}
}
}
if (objPoints != NULL) {
printf("construct body\n");
b2PolygonShape cs;
b2Vec2 vertices[4] = {
b2Vec2((float)(objPoints[0].x)/WORLD_SCALE, (float)(objPoints[0].y)/WORLD_SCALE),
b2Vec2((float)(objPoints[1].x)/WORLD_SCALE, (float)(objPoints[1].y)/WORLD_SCALE),
b2Vec2((float)(objPoints[2].x)/WORLD_SCALE, (float)(objPoints[2].y)/WORLD_SCALE),
b2Vec2((float)(objPoints[3].x)/WORLD_SCALE, (float)(objPoints[3].y)/WORLD_SCALE)
};
cs.Set(vertices, 4);
b2BodyDef bd;
//bd.type = b2_staticBody;
Box2DData* obj = new Box2DData();
strcpy(obj->type, "Box");
bd.userData = obj;
b2Body* body1 = world->CreateBody(&bd);
body1->CreateFixture(&cs, 0.0f);
}
if (_r.x < 0) return;
Point2D r = toPoint2D(_r);
// if marker is not moving for a while, reset the path
int len = path.size();
if (len > KEEP_MAX) {
path.erase(path.begin());
}
int nearCount = 0;
int actual = min(KEEP_COUNT, len);
/*
for(int i=0; i<actual; i++){
Point2D p = path[len-1-i];
double d = dist(p, r);
//printf("dist=%f\n", d);
if (d < NEAR_THRESHOLD) ++nearCount;
}
if (nearCount > (double)actual * DONT_MOVE_THRESHOLD_RATE) {
// marker is not moving, so clear the path
printf("cleared\n");
path.clear();
}
*/
path.push_back(r);
// decide if we should recognize
time_t current;
time(¤t);
double interval = difftime(current, lastTime);
printf("interval=%f\n", interval);
if (interval < INTERVAL_SEC) return;
len = path.size();
if (len < 5) return;
RecognitionResult res = g.recognize(path);
printf("%s:%f\n", res.name.c_str(), res.score);
if (res.name == "Circle" && res.score > SCORE_THRESHOLD) {
printf("##circle detect##\n");
// convert to vector<Point2D> to CvSeq<CvPoint>
CvSeqWriter writer;
CvMemStorage* storage = cvCreateMemStorage(0);
cvStartWriteSeq( CV_32SC2, sizeof(CvSeq), sizeof(CvPoint), storage, &writer);
for (int i=0; i<len; i++) {
CvPoint pt = toCvPoint(path[i]);
CV_WRITE_SEQ_ELEM(pt, writer);
}
CvSeq* seq = cvEndWriteSeq(&writer);
CvBox2D ellipse = cvFitEllipse2(seq);
float radius = std::min(ellipse.size.width, ellipse.size.height)/(4.0F*WORLD_SCALE);
cvEllipseBox(destImg, ellipse, CV_RGB(0,255,255), -1);
// add Box2D object
{
b2CircleShape cs;
cs.m_radius = radius;
printf(" x=%f y=%f radius:%f\n", ellipse.center.x/WORLD_SCALE, ellipse.center.y/WORLD_SCALE, radius);
b2BodyDef bd;
bd.type = b2_dynamicBody;
bd.position.Set(ellipse.center.x/WORLD_SCALE, ellipse.center.y/WORLD_SCALE);
Box2DData* obj = new Box2DData();
strcpy(obj->type, "Circle");
obj->radius = radius;
bd.userData = obj;
b2Body* body1 = world->CreateBody(&bd);
b2FixtureDef fixtureDef;
fixtureDef.shape = &cs;
fixtureDef.density = 1.0f;
fixtureDef.friction = 0.3f;
fixtureDef.restitution = 0.6f;
body1->CreateFixture(&fixtureDef);
}
time(&lastTime);
//cvEllipseBox(destImg, ellipse, CV_RGB(125,125,255));
}
}
