// hmmm... copied from PPlot.cpp
void PZoomInteraction::DoZoomIn (float inX1, float inX2, float inY1, float inY2) {
if (!CheckRange (inX1, inX2)) {
return;
}
if (!CheckRange (inY1, inY2)) {
return;
}
// also use the following criterium that is used in PPlot::CalculateTickInfo to
// avoid strange zoom in / zoom out behaviour
float theYRange = fabs (inY1 - inY2);
float theYMax = (inY1 > inY2) ? inY1 : inY2;
if (fabs (theYRange / theYMax) < PPlot::kRangeVerySmall) {
return;
}
StoreCurrentAxisSetup ();
if (mZoomHistory.size()==1)
mOriginalAxis = mZoomHistory.top();
if (IsZoomRegion () || IsZoomX ()) {
mPPlot.mXAxisSetup.SetAutoScale (false);
mPPlot.mXAxisSetup.mMin = pmin (inX1, inX2);
mPPlot.mXAxisSetup.mMax = pmax (inX1, inX2);
}
if (IsZoomRegion () || IsZoomY ()) {
mPPlot.mYAxisSetup.SetAutoScale (false);
mPPlot.mYAxisSetup.mMin = pmin (inY1, inY2);
mPPlot.mYAxisSetup.mMax = pmax (inY1, inY2);
}
mLastZoomIn = true;
return;
}
CSGeometry* NGInterface::loadCSG(string data)
{
strstream in;
CSGeometry *geom;
Point<3> pmin(minX,minY,minZ),
pmax(maxX,maxY,maxZ);
Box<3> box(pmin,pmax);
in << data;
try
{
geom = ParseCSG(in);
}
catch(...)
{
cerr << "Problem in CSG-file!" << endl;
return NULL;
}
if (!geom)
{
cout << "geo-file should start with 'algebraic3d'" << endl;
return NULL;
}
else
geom->FindIdenticSurfaces(1e-8*geom->MaxSize());
geom->SetBoundingBox(box);
geom->CalcTriangleApproximation(detail,facets);
return (CSGeometry*)(geometry = geom);
}
Position* RobotMemory::find(int type)
{
Position pmin(0,0);
Position pmax(0,0);
pmin.x = owner->getPosition()->x;
pmin.y = owner->getPosition()->y;
pmax.x = pmin.x;
pmax.y = pmin.y;
while((pmin.x > 0) || (pmin.y > 0) || (pmax.x < columns) || (pmax.y < rows))
{
pmin.x --;
pmin.y --;
pmax.x ++;
pmax.y ++;
for(int i=pmin.x; i<=pmax.x; i++)
for(int j=pmin.y; j<=pmax.y; j++)
if (!((i < 0) || (i >= columns) || (j < 0) || (j >= rows)))
if(map[i][j] == type)
{
TRACE("ZNALEZIONO OBIEKT. XY = %d %d \n",i,j);
return new Position(i,j);
}
}
return new Position(-1,-1);
}
void fdjac2(custom_funcmult *funcmult, double *x, int M, int N, double *fvec, double *fjac, int ldfjac,
double epsfcn,double eps) {
int i,j;
double epsmch,h,temp,zero;
double *wa;
zero = 0.0;
epsmch = eps;
eps = sqrt(pmax(epsfcn,epsmch));
wa = (double*) malloc(sizeof(double) *M);
for(j = 0; j < N; ++j) {
temp = x[j];
h = eps*fabs(temp);
if (h == zero) {
h = eps;
}
x[j] = temp + h;
FUNCMULT_EVAL(funcmult,x,M,N,wa);
x[j] = temp;
for(i = 0; i < M; ++i) {
fjac[i*N+j] = (wa[i] - fvec[i])/h;
}
}
free(wa);
}
/// @brief OBB merge method when the centers of two smaller OBB are close
inline OBB merge_smalldist(const OBB& b1, const OBB& b2)
{
OBB b;
b.To = (b1.To + b2.To) * 0.5;
Quaternion3f q0, q1;
q0.fromAxes(b1.axis);
q1.fromAxes(b2.axis);
if(q0.dot(q1) < 0)
q1 = -q1;
Quaternion3f q = q0 + q1;
FCL_REAL inv_length = 1.0 / std::sqrt(q.dot(q));
q = q * inv_length;
q.toAxes(b.axis);
Vec3f vertex[8], diff;
FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
Vec3f pmin(real_max, real_max, real_max);
Vec3f pmax(-real_max, -real_max, -real_max);
computeVertices(b1, vertex);
for(int i = 0; i < 8; ++i)
{
diff = vertex[i] - b.To;
for(int j = 0; j < 3; ++j)
{
FCL_REAL dot = diff.dot(b.axis[j]);
if(dot > pmax[j])
pmax[j] = dot;
else if(dot < pmin[j])
pmin[j] = dot;
}
}
computeVertices(b2, vertex);
for(int i = 0; i < 8; ++i)
{
diff = vertex[i] - b.To;
for(int j = 0; j < 3; ++j)
{
FCL_REAL dot = diff.dot(b.axis[j]);
if(dot > pmax[j])
pmax[j] = dot;
else if(dot < pmin[j])
pmin[j] = dot;
}
}
for(int j = 0; j < 3; ++j)
{
b.To += (b.axis[j] * (0.5 * (pmax[j] + pmin[j])));
b.extent[j] = 0.5 * (pmax[j] - pmin[j]);
}
return b;
}
void MAPOBJECT::PaintSelected()
{
if(!m_selected || m_pDevice == NULL)return;
BBOX bbox = GetBoundingBox(); //Bounding box in world space
// Create 8 points according to the corners of the bounding box
D3DXVECTOR3 corners[] = {D3DXVECTOR3(bbox.max.x, bbox.max.y, bbox.max.z),
D3DXVECTOR3(bbox.max.x, bbox.max.y, bbox.min.z),
D3DXVECTOR3(bbox.max.x, bbox.min.y, bbox.max.z),
D3DXVECTOR3(bbox.max.x, bbox.min.y, bbox.min.z),
D3DXVECTOR3(bbox.min.x, bbox.max.y, bbox.max.z),
D3DXVECTOR3(bbox.min.x, bbox.max.y, bbox.min.z),
D3DXVECTOR3(bbox.min.x, bbox.min.y, bbox.max.z),
D3DXVECTOR3(bbox.min.x, bbox.min.y, bbox.min.z)};
// Find the max and min points of these
// 8 offests points in screen space
INTPOINT pmax(-10000, -10000), pmin(10000,10000);
for(int i=0;i<8;i++)
{
INTPOINT screenPos = GetScreenPos(corners[i], m_pDevice);
if(screenPos.x > pmax.x)pmax.x = screenPos.x;
if(screenPos.y > pmax.y)pmax.y = screenPos.y;
if(screenPos.x < pmin.x)pmin.x = screenPos.x;
if(screenPos.y < pmin.y)pmin.y = screenPos.y;
}
RECT scr = {-20, -20, 820, 620};
// Check that the max and min point is within our viewport boundaries
if(pmax.inRect(scr) || pmin.inRect(scr))
{
float s = (pmax.x - pmin.x) / 3.0f;
if((pmax.y - pmin.y) < (pmax.x - pmin.x))s = (pmax.y - pmin.y) / 3.0f;
D3DXVECTOR2 corner1[] = {D3DXVECTOR2((float)pmin.x, (float)pmin.y + s), D3DXVECTOR2((float)pmin.x, (float)pmin.y), D3DXVECTOR2((float)pmin.x + s, (float)pmin.y)};
D3DXVECTOR2 corner2[] = {D3DXVECTOR2((float)pmax.x - s, (float)pmin.y), D3DXVECTOR2((float)pmax.x, (float)pmin.y), D3DXVECTOR2((float)pmax.x, (float)pmin.y + s)};
D3DXVECTOR2 corner3[] = {D3DXVECTOR2((float)pmax.x, (float)pmax.y - s), D3DXVECTOR2((float)pmax.x, (float)pmax.y), D3DXVECTOR2((float)pmax.x - s, (float)pmax.y)};
D3DXVECTOR2 corner4[] = {D3DXVECTOR2((float)pmin.x + s, (float)pmax.y), D3DXVECTOR2((float)pmin.x, (float)pmax.y), D3DXVECTOR2((float)pmin.x, (float)pmax.y - s)};
//Draw the 4 corners
if(line != NULL)
{
line->SetWidth(2.0f);
line->Begin();
line->Draw(corner1, 3, 0xffffffff);
line->Draw(corner2, 3, 0xffffffff);
line->Draw(corner3, 3, 0xffffffff);
line->Draw(corner4, 3, 0xffffffff);
line->End();
}
}
}
void pigalePaint::update(int i,bool newDrawing)
{zoom = 1;
index = i;
if(newDrawing) // copy the graph
GCP = mw->GC;
TopologicalGraph G(GCP);
Prop1<Tpoint> pmin(G.Set(),PROP_POINT_MIN);
Prop1<Tpoint> pmax(G.Set(),PROP_POINT_MAX);
xmin = pmin().x(); ymin = pmin().y();
xmax = pmax().x(); ymax = pmax().y();
Wx_max = this->width() - 2*border; Wy_max = this->height() - 2*border;
xscale0 = xscale = Wx_max/(xmax - xmin);
xtr0 = xtr = - xmin*xscale + border;
yscale0 = yscale = Wy_max/(ymax - ymin);
ytr0 = ytr = - ymin*yscale +border;
mw->tabWidget->setTabText(mw->tabWidget->indexOf(this)
,qApp->translate("pigalePaint",DrawFunctions[index].name));
mw->tabWidget->setCurrentIndex(mw->tabWidget->indexOf(this));
}
inline
void findPeak(double *idata, place * odata, int siz, int pitch) {
place p = {0u, 0.0};
zeroPad(idata, siz, pitch);
unsigned int n = siz * pitch;
#pragma omp declare reduction (maxer: place : omp_out = pmax(omp_out, omp_in))
#pragma omp parallel for reduction(maxer: p)
for (unsigned int i = 0; i<n; i++) {
p = pmax(p, {n, fabs(idata[n])});
}
*odata = p;
}
// arguments allow us to zoom out in X direction but change the Y-axis
void PZoomInteraction::DoZoomOut (float inY1, float inY2) {
if (mZoomHistory.size () == 0) {
return;
}
PAxisInfo theInfo = mZoomHistory.top ();
mZoomHistory.pop ();
mPPlot.mXAxisSetup = theInfo.mXAxisSetup;
if (inY1 != -1) {
mPPlot.mYAxisSetup.mMin = pmin (inY1, inY2);
mPPlot.mYAxisSetup.mMax = pmax (inY1, inY2);
} else {
mPPlot.mYAxisSetup = theInfo.mYAxisSetup;
}
mLastZoomIn = false;
}
void DrawTriangle(QPainter *p,pigalePaint *paint)
{TopologicalGraph G(paint->GCP);
Prop1<Tpoint> pmin(G.Set(),PROP_POINT_MIN);
Prop1<Tpoint> pmax(G.Set(),PROP_POINT_MAX);
Prop<Tpoint> pleft(G.Set(tvertex()),PROP_DRAW_POINT_1);
Prop<Tpoint> pright(G.Set(tvertex()),PROP_DRAW_POINT_2);
Prop<Tpoint> ptop(G.Set(tvertex()),PROP_DRAW_POINT_3);
Prop<short> vcolor(G.Set(tvertex()),PROP_COLOR);
p->setFont(QFont("sans",Min((int)(Min(paint->xscale,paint->yscale) + .5),13)));
for(tvertex iv = 1; iv <= G.nv();iv++)
{paint->DrawTriangle(p,pleft[iv],pright[iv],ptop[iv],vcolor[iv]);
Tpoint center = (pleft[iv]+pright[iv]+ptop[iv])/3.;
paint->DrawText(p,center,iv,vcolor[iv],1);
}
}
static double expShulen(double T, /* absolute error */
double M, /* min logarithm */
double d,
double p,
GtUword subjectLength,
double *ln_n_fac,
double *s1,
GtUword n_s)
{
GtUword i;
int thresholdReached = 0;
double prob_i, probOld, delta, factor;
double e = 0.0; /* expectation */
double t = 1.0 - d;
double p_t = t;
probOld = 0.0;
/*since for i = 0, the whole expression is 0*/
for (i = 1LU; i < subjectLength; i++) {
factor = 1.0 - p_t;
if (!thresholdReached) {
prob_i = factor * pmax(M, i, p,
subjectLength,
&thresholdReached,
ln_n_fac,
s1,
n_s);
}
else
prob_i = factor; /* prob_i = factor * s, where s = 1 */
delta = (prob_i - probOld) * i; /* delta should always be positive */
gt_assert(gt_double_equals_double(delta, 0.0) ||
gt_double_larger_double(delta, 0.0));
e += delta; /* expectation of avg shulen length(Q, S) */
/* check error */
if (1.0 <= e && delta / e <= T)
break;
p_t *= t;
probOld = prob_i;
}
return e;
}
size_t write_array_slice_u16(
lua_State *L,
int table_index,
u16 *data,
v3u16 data_size,
v3u16 slice_offset,
v3u16 slice_size)
{
v3u16 pmin, pmax(data_size);
if (slice_offset.X > 0) {
slice_offset.X--;
pmin.X = slice_offset.X;
pmax.X = MYMIN(slice_offset.X + slice_size.X, data_size.X);
}
if (slice_offset.Y > 0) {
slice_offset.Y--;
pmin.Y = slice_offset.Y;
pmax.Y = MYMIN(slice_offset.Y + slice_size.Y, data_size.Y);
}
if (slice_offset.Z > 0) {
slice_offset.Z--;
pmin.Z = slice_offset.Z;
pmax.Z = MYMIN(slice_offset.Z + slice_size.Z, data_size.Z);
}
const u32 ystride = data_size.X;
const u32 zstride = data_size.X * data_size.Y;
u32 elem_index = 1;
for (u32 z = pmin.Z; z != pmax.Z; z++)
for (u32 y = pmin.Y; y != pmax.Y; y++)
for (u32 x = pmin.X; x != pmax.X; x++) {
u32 i = z * zstride + y * ystride + x;
lua_pushinteger(L, data[i]);
lua_rawseti(L, table_index, elem_index);
elem_index++;
}
return elem_index - 1;
}
void OrthoBrick :: Reduce (const BoxSphere<3> & box)
{
surfaceactive.Elem(1) =
(box.PMin()(2) < pmin(2)) && (pmin(2) < box.PMax()(2));
surfaceactive.Elem(2) =
(box.PMin()(2) < pmax(2)) && (pmax(2) < box.PMax()(2));
surfaceactive.Elem(3) =
(box.PMin()(1) < pmin(1)) && (pmin(1) < box.PMax()(1));
surfaceactive.Elem(4) =
(box.PMin()(1) < pmax(1)) && (pmax(1) < box.PMax()(1));
surfaceactive.Elem(5) =
(box.PMin()(0) < pmin(0)) && (pmin(0) < box.PMax()(0));
surfaceactive.Elem(6) =
(box.PMin()(0) < pmax(0)) && (pmax(0) < box.PMax()(0));
}
INSOLID_TYPE OrthoBrick :: BoxInSolid (const BoxSphere<3> & box) const
{
if (pmin(0) > box.PMax()(0) ||
pmin(1) > box.PMax()(1) ||
pmin(2) > box.PMax()(2) ||
pmax(0) < box.PMin()(0) ||
pmax(1) < box.PMin()(1) ||
pmax(2) < box.PMin()(2))
return IS_OUTSIDE;
if (pmin(0) < box.PMin()(0) &&
pmin(1) < box.PMin()(1) &&
pmin(2) < box.PMin()(2) &&
pmax(0) > box.PMax()(0) &&
pmax(1) > box.PMax()(1) &&
pmax(2) > box.PMax()(2))
return IS_INSIDE;
return DOES_INTERSECT;
}
bool ofxOBJModel::load(string path) {
bHasNormals = false;
bHasTexCoords = false;
filePath = path;
path = ofToDataPath(path, true);
string line;
for(int i = 0; i < meshes.size(); i++) {
delete meshes[i];
}
meshes.clear();
ObjMesh *currMesh = NULL;
// this is a list of all points
// that we can drop after parsing
vector<ofPoint> points;
vector<ofPoint> normals;
vector<ofPoint> texCoords;
// obj file format vertexes are 1-indexed
points.push_back(ofPoint());
normals.push_back(ofPoint());
texCoords.push_back(ofPoint());
ifstream myfile (path.c_str());
if (myfile.is_open()) {
while (! myfile.eof()) {
getline (myfile,line);
// parse the obj format here.
//
// the only things we're interested in is
// lines beginning with 'g' - this says start of new object
// lines beginning with 'v ' - coordinate of a vertex
// lines beginning with 'vn ' - vertex normals -- todo
// lines beginning with 'vt ' - texcoords (either 2 or 3 values) -- todo
// lines beginning with 'f ' - specifies a face of a shape
// we take each number before the slash as the index
// of the vertex to join up to create a face.
if(line.find("g ")==0) { // new object definition
currMesh = new ObjMesh(line.substr(2));
meshes.push_back(currMesh);
} else if(line.find("v ")==0) { // new vertex
points.push_back(parseCoords(line));
} else if(line.find("vn ")==0) {
bHasNormals = true;
normals.push_back(parseCoords(line));
} else if(line.find("vt ")==0) {
bHasTexCoords = true;
texCoords.push_back(parseCoords(line));
} else if(line.find("f ")==0) { // face definition
if(currMesh!=NULL) {
line = line.substr(2); // lop off "f "
vector<string> indices = split(line, ' ');
// remove any texcoords (/xxx's)
ObjFace *face = new ObjFace();
for(int i = 0; i < indices.size(); i++) {
vector<string> parts = ofSplitString(indices[i], "/");
// first index is always a point
face->points.push_back(points[atoi(parts[0].c_str())]);
if(parts.size()==2) {
face->texCoords.push_back(texCoords[atoi(parts[1].c_str())]);
} else if(parts.size()==3) {
face->normals.push_back(normals[atoi(parts[2].c_str())]);
if(parts[1]!="") {
face->texCoords.push_back(texCoords[atoi(parts[1].c_str())]);
}
}
}
currMesh->addFace(face);
}
}
}
myfile.close();
//#define NORMALIZE_TEXCOORDS
#ifdef NORMALIZE_TEXCOORDS
ofPoint pmin(FLT_MAX, FLT_MAX);
ofPoint pmax(FLT_MIN, FLT_MIN);
for(int i = 0; i < texCoords.size(); i++) {
if(texCoords[i].x<pmin.x) pmin.x = texCoords[i].x;
if(texCoords[i].y<pmin.y) pmin.y = texCoords[i].y;
if(texCoords[i].x>pmax.x) pmax.x = texCoords[i].x;
if(texCoords[i].y>pmax.y) pmax.y = texCoords[i].y;
}
for(int k = 0; k < meshes.size(); k++)
for(int i = 0; i < meshes[k]->faces.size(); i++)
for(int j = 0; j < meshes[k]->faces[i]->texCoords.size(); j++) {
ofPoint p = meshes[k]->faces[i]->texCoords[j];
p.x = ofMap(p.x, pmin.x, pmax.x, 0, 1);
p.y = ofMap(p.y, pmin.y, pmax.y, 0, 1);
meshes[k]->faces[i]->texCoords[j] = p;
}
#endif
loadVbo();
ofLog(OF_LOG_NOTICE, "Successfully loaded %s\n-----\nVertices: %d\nMeshes: %d\nNormals: %d\nTexCoords: %d\n", path.c_str(), points.size(), meshes.size(), normals.size(), texCoords.size());
return true;
} else {
ofLog(OF_LOG_ERROR, "Couldn't find the OBJ file %s\n", path.c_str());
return false;
}
}
FIT_TEST_CASE()
{
FIT_TEST_CHECK(pmax(3, 4) == std::max(3, 4));
FIT_TEST_CHECK(psum(1, 2) == 3);
}
void Grid::traceCoherent(const vector<Segment> &segments, vector<pair<int, float>> &out, int ignored_id, int flags) const {
out.resize(segments.size(), make_pair(-1, constant::inf));
if(segments.empty())
return;
int2 start, end; {
float3 pmin(constant::inf, constant::inf, constant::inf);
float3 pmax(-constant::inf, -constant::inf, -constant::inf);
for(int s = 0; s < (int)segments.size(); s++) {
const Segment &segment = segments[s];
float tmin = max(0.0f, intersection(segment, m_bounding_box));
float tmax = min(segment.length(), -intersection(-segment, m_bounding_box));
float3 p1 = segment.at(tmin), p2 = segment.at(tmax);
pmin = min(pmin, min(p1, p2));
pmax = max(pmax, max(p1, p2));
}
start = worldToGrid((int2)pmin.xz());
end = worldToGrid((int2)pmax.xz());
start = max(start, int2(0, 0));
end = min(end, int2(m_size.x - 1, m_size.y - 1));
}
float max_dist = -constant::inf;
Interval idir[3], origin[3]; {
const Segment &first = segments.front();
idir [0] = first.invDir().x; idir [1] = first.invDir().y; idir [2] = first.invDir().z;
origin[0] = first.origin().x; origin[1] = first.origin().y; origin[2] = first.origin().z;
for(int s = 1; s < (int)segments.size(); s++) {
const Segment &segment = segments[s];
float tidir[3] = { segment.invDir().x, segment.invDir().y, segment.invDir().z };
float torigin[3] = { segment.origin().x, segment.origin().y, segment.origin().z };
max_dist = max(max_dist, segment.length());
for(int i = 0; i < 3; i++) {
idir [i] = Interval(min(idir [i].min, tidir [i]), max(idir [i].max, tidir [i]));
origin[i] = Interval(min(origin[i].min, torigin[i]), max(origin[i].max, torigin[i]));
}
}
}
for(int x = start.x; x <= end.x; x++) for(int z = start.y; z <= end.y; z++) {
int node_id = nodeAt(int2(x, z));
const Node &node = m_nodes[node_id];
if(flagTest(node.obj_flags, flags) && intersection(idir, origin, node.bbox) < max_dist) {
const Object *objects[node.size];
int count = extractObjects(node_id, objects, ignored_id, flags);
for(int n = 0; n < count; n++) {
if(intersection(idir, origin, objects[n]->bbox) < max_dist) {
for(int s = 0; s < (int)segments.size(); s++) {
const Segment &segment = segments[s];
float dist = intersection(segment, objects[n]->bbox);
if(dist < out[s].second) {
out[s].second = dist;
out[s].first = objects[n] - &m_objects[0];
}
}
}
}
if(node.is_dirty)
updateNode(node_id);
}
}
}
void MeshFromSpline2D (SplineGeometry2d & geometry,
Mesh *& mesh,
MeshingParameters & mp)
{
PrintMessage (1, "Generate Mesh from spline geometry");
double h = mp.maxh;
Box<2> bbox = geometry.GetBoundingBox ();
if (bbox.Diam() < h)
{
h = bbox.Diam();
mp.maxh = h;
}
mesh = new Mesh;
mesh->SetDimension (2);
geometry.PartitionBoundary (h, *mesh);
// marks mesh points for hp-refinement
for (int i = 0; i < geometry.GetNP(); i++)
if (geometry.GetPoint(i).hpref)
{
double mindist = 1e99;
PointIndex mpi(0);
Point<2> gp = geometry.GetPoint(i);
Point<3> gp3(gp(0), gp(1), 0);
for (PointIndex pi = PointIndex::BASE;
pi < mesh->GetNP()+PointIndex::BASE; pi++)
if (Dist2(gp3, (*mesh)[pi]) < mindist)
{
mpi = pi;
mindist = Dist2(gp3, (*mesh)[pi]);
}
(*mesh)[mpi].Singularity(1.);
}
int maxdomnr = 0;
for (SegmentIndex si = 0; si < mesh->GetNSeg(); si++)
{
if ( (*mesh)[si].domin > maxdomnr) maxdomnr = (*mesh)[si].domin;
if ( (*mesh)[si].domout > maxdomnr) maxdomnr = (*mesh)[si].domout;
}
mesh->ClearFaceDescriptors();
for (int i = 1; i <= maxdomnr; i++)
mesh->AddFaceDescriptor (FaceDescriptor (i, 0, 0, i));
// set Array<string*> bcnames...
// number of bcnames
int maxsegmentindex = 0;
for (SegmentIndex si = 0; si < mesh->GetNSeg(); si++)
{
if ( (*mesh)[si].si > maxsegmentindex) maxsegmentindex = (*mesh)[si].si;
}
mesh->SetNBCNames(maxsegmentindex);
for ( int sindex = 0; sindex < maxsegmentindex; sindex++ )
mesh->SetBCName ( sindex, geometry.GetBCName( sindex+1 ) );
for (SegmentIndex si = 0; si < mesh->GetNSeg(); si++)
(*mesh)[si].SetBCName ( (*mesh).GetBCNamePtr( (*mesh)[si].si-1 ) );
Point3d pmin(bbox.PMin()(0), bbox.PMin()(1), -bbox.Diam());
Point3d pmax(bbox.PMax()(0), bbox.PMax()(1), bbox.Diam());
mesh->SetLocalH (pmin, pmax, mparam.grading);
mesh->SetGlobalH (h);
mesh->CalcLocalH();
int bnp = mesh->GetNP(); // boundary points
int hquad = mparam.quad;
for (int domnr = 1; domnr <= maxdomnr; domnr++)
if (geometry.GetDomainTensorMeshing (domnr))
{ // tensor product mesh
Array<PointIndex, PointIndex::BASE> nextpi(bnp);
Array<int, PointIndex::BASE> si1(bnp), si2(bnp);
PointIndex firstpi;
nextpi = -1;
si1 = -1;
si2 = -1;
for (SegmentIndex si = 0; si < mesh->GetNSeg(); si++)
{
int p1 = -1, p2 = -2;
if ( (*mesh)[si].domin == domnr)
{ p1 = (*mesh)[si][0]; p2 = (*mesh)[si][1]; }
if ( (*mesh)[si].domout == domnr)
{ p1 = (*mesh)[si][1]; p2 = (*mesh)[si][0]; }
if (p1 == -1) continue;
nextpi[p1] = p2; // counter-clockwise
int index = (*mesh)[si].si;
if (si1[p1] != index && si2[p1] != index)
{ si2[p1] = si1[p1]; si1[p1] = index; }
if (si1[p2] != index && si2[p2] != index)
{ si2[p2] = si1[p2]; si1[p2] = index; }
}
PointIndex c1(0), c2, c3, c4; // 4 corner points
int nex = 1, ney = 1;
for (PointIndex pi = 1; pi <= si2.Size(); pi++)
if (si2[pi] != -1)
{ c1 = pi; break; }
for (c2 = nextpi[c1]; si2[c2] == -1; c2 = nextpi[c2], nex++);
for (c3 = nextpi[c2]; si2[c3] == -1; c3 = nextpi[c3], ney++);
for (c4 = nextpi[c3]; si2[c4] == -1; c4 = nextpi[c4]);
Array<PointIndex> pts ( (nex+1) * (ney+1) ); // x ... inner loop
pts = -1;
for (PointIndex pi = c1, i = 0; pi != c2; pi = nextpi[pi], i++)
pts[i] = pi;
for (PointIndex pi = c2, i = 0; pi != c3; pi = nextpi[pi], i++)
pts[(nex+1)*i+nex] = pi;
for (PointIndex pi = c3, i = 0; pi != c4; pi = nextpi[pi], i++)
pts[(nex+1)*(ney+1)-i-1] = pi;
for (PointIndex pi = c4, i = 0; pi != c1; pi = nextpi[pi], i++)
pts[(nex+1)*(ney-i)] = pi;
for (PointIndex pix = nextpi[c1], ix = 0; pix != c2; pix = nextpi[pix], ix++)
for (PointIndex piy = nextpi[c2], iy = 0; piy != c3; piy = nextpi[piy], iy++)
{
Point<3> p = (*mesh)[pix] + ( (*mesh)[piy] - (*mesh)[c2] );
pts[(nex+1)*(iy+1) + ix+1] = mesh -> AddPoint (p , 1, FIXEDPOINT);
}
for (int i = 0; i < ney; i++)
for (int j = 0; j < nex; j++)
{
Element2d el(QUAD);
el[0] = pts[i*(nex+1)+j];
el[1] = pts[i*(nex+1)+j+1];
el[2] = pts[(i+1)*(nex+1)+j+1];
el[3] = pts[(i+1)*(nex+1)+j];
el.SetIndex (domnr);
mesh -> AddSurfaceElement (el);
}
}
for (int domnr = 1; domnr <= maxdomnr; domnr++)
{
if (geometry.GetDomainTensorMeshing (domnr)) continue;
if ( geometry.GetDomainMaxh ( domnr ) > 0 )
h = geometry.GetDomainMaxh(domnr);
PrintMessage (3, "Meshing domain ", domnr, " / ", maxdomnr);
int oldnf = mesh->GetNSE();
mparam.quad = hquad || geometry.GetDomainQuadMeshing (domnr);
Meshing2 meshing (Box<3> (pmin, pmax));
for (PointIndex pi = PointIndex::BASE; pi < bnp+PointIndex::BASE; pi++)
meshing.AddPoint ( (*mesh)[pi], pi);
PointGeomInfo gi;
gi.trignum = 1;
for (SegmentIndex si = 0; si < mesh->GetNSeg(); si++)
{
if ( (*mesh)[si].domin == domnr)
meshing.AddBoundaryElement ( (*mesh)[si][0] + 1 - PointIndex::BASE,
(*mesh)[si][1] + 1 - PointIndex::BASE, gi, gi);
if ( (*mesh)[si].domout == domnr)
meshing.AddBoundaryElement ( (*mesh)[si][1] + 1 - PointIndex::BASE,
(*mesh)[si][0] + 1 - PointIndex::BASE, gi, gi);
}
mparam.checkoverlap = 0;
meshing.GenerateMesh (*mesh, h, domnr);
for (SurfaceElementIndex sei = oldnf; sei < mesh->GetNSE(); sei++)
(*mesh)[sei].SetIndex (domnr);
// astrid
char * material;
geometry.GetMaterial( domnr, material );
if ( material )
{
(*mesh).SetMaterial ( domnr, material );
}
}
mparam.quad = hquad;
int hsteps = mp.optsteps2d;
mp.optimize2d = "smcm";
mp.optsteps2d = hsteps/2;
Optimize2d (*mesh, mp);
mp.optimize2d = "Smcm";
mp.optsteps2d = (hsteps+1)/2;
Optimize2d (*mesh, mp);
mp.optsteps2d = hsteps;
mesh->Compress();
mesh -> SetNextMajorTimeStamp();
extern void Render();
Render();
}
int EmbedVision(TopologicalGraph &G)
{int morg = G.ne();
if(!G.CheckConnected())G.MakeConnected();
if(!G.FindPlanarMap())
{Tprintf("Not Planar Graph");
for(tedge e = G.ne(); e > morg; e--) G.DeleteEdge(e);
return -1;
}
if(!G.CheckBiconnected())G.Biconnect();
bool alreadyBipolarOriented = false;
bool stConnected = false;
tvertex s,t;
tbrin bs,bt;
bool already2Connected = (morg == G.ne());
if(already2Connected && (alreadyBipolarOriented = CheckBipolarlyOriented(G,s,t,stConnected,bs,true)) == true)
{if(stConnected)Tprintf("Using original orientation (s,t are connected)");
else Tprintf("Using original orientation (s,t are not connected)");
bt = -bs;
}
else
{// Find reasonable s t vertices
tedge e;
tbrin b;
int len;
G.LongestFace(bs,len);
s = G.vin[bs];
bt = bs;
for(int i = 1 ;i <= len/2; i++)bt = G.cir[-bt];
t = G.vin[bt];
// Check if s an t are connected
b = bs;
do
{if(G.vin[-b] == t){stConnected = true;break;}
}while((b = G.cir[b]) != bs);
if(!stConnected)
{G.NewEdge(bs,bt);
bs = (tbrin)G.ne();
}
s = G.vin[bs]; t = G.vin[-bs];
// BipolarOrient the graph
G.BipolarPlan(bs);
G.FixOrientation();
}
int n = G.nv(); int m = G.ne();
// if bs has been reoriented as the packing suppose that vin[bst]= source
tbrin bst = (G.vin[bs] != s) ? -bs : bs;
// Compute y coords
Prop<int> y(G.Set(tvertex()),PROP_DRAW_INT_5); y.clear();
MaxPath *MP=new MaxPath(n,m);
for(tedge e = 1; e <= m; e++)
MP->insert(G.vin[e.firsttbrin()](),G.vin[e.secondtbrin()](),1);
MP->solve(y);
delete MP;
int maxyval = y[t];
// compute MaxPath for edges
svector<int> x(0,m); x.clear();
MP=new MaxPath(m,2*m);
svector<tbrin> &Fpbrin = G.ComputeFpbrin();
tbrin b0,b;
// out == positif
for(int i = 1; i <= Fpbrin.n(); i++)
{b0 = Fpbrin[i];
if(b0.out())
while((b=-G.acir[b0]).out())
b0=b;
else
do
{b0=G.cir[-b0];}
while(b0.in());
// b0 is the lowest tbrin on the left of the face
if(b0 == G.cir[bst])continue; // face exterieure
// référence : e
tedge e = (G.acir[b0]).GetEdge();
b=b0;
while (b.out())
{if(stConnected || b != bst)
MP->insert(b.GetEdge()(),e(),1);
b=G.cir[-b];
}
while (b.GetEdge()!=e)
{MP->insert(e(),b.GetEdge()(),0);
b=G.cir[-b];
}
}
MP->solve(x);
delete &Fpbrin;
delete MP;
// computes extremities of vertices
Prop<int> x1(G.Set(tvertex()),PROP_DRAW_INT_1);
Prop<int> x2(G.Set(tvertex()),PROP_DRAW_INT_2);
int maxxval=ComputeExtremities(G,x,x1,x2,morg);
Prop1<int> m_org(G.Set(),PROP_TMP);
m_org() = morg;
for(tedge e = G.ne(); e > morg; e--)
G.DeleteEdge(e);
Prop1<int> maxx(G.Set(),PROP_DRAW_INT_1);
Prop1<int> maxy(G.Set(),PROP_DRAW_INT_2);
maxx()=maxxval; maxy()=maxyval;
Prop1<Tpoint> pmin(G.Set(),PROP_POINT_MIN);
Prop1<Tpoint> pmax(G.Set(),PROP_POINT_MAX);
pmin() = Tpoint(-1,-1);
pmax() = Tpoint(maxxval+1,maxyval+1);
Prop<Tpoint> P1(G.Set(tedge()),PROP_DRAW_POINT_1);
Prop<Tpoint> P2(G.Set(tedge()),PROP_DRAW_POINT_2);
for (tedge e=1; e<= G.ne(); e++)
{P1[e]=Tpoint(x[e],y[G.vin[e.firsttbrin()]]);
P2[e]=Tpoint(x[e],y[G.vin[e.secondtbrin()]]);
}
return 0;
}
int cvsrch(custom_function *funcpt, custom_gradient *funcgrad, double *x, double *f, double *g, double *stp, double *s, int N, double *dx, double maxstep,
int MAXITER,double eps2,double ftol, double gtol, double xtol) {
int info,i,siter,nfev;
int infoc, j, brackt, stage1;
double dg, dgm, dginit, dgtest, dgx, dgxm, dgy, dgym, finit, ftest1, fm, fx, fxm, fy, fym, p5, p66, stx, sty,
stmin, stmax, width, width1, xtrapf;
double nlen,den,rell,stepmin;
double *rcheck,*wa;
rcheck = (double*)malloc(sizeof(double)*N);
wa = (double*)malloc(sizeof(double)*N);
nlen = 0.0;
p5 = 0.5;
p66 = 0.66;
xtrapf = 4.0;
info = 0;
infoc = 1;
siter = MAXITER;
if (N <= 0 || *stp <= 0.0 || ftol < 0.0 || gtol < 0.0 || xtol < 0.0) {
return info;
}
for (i = 0; i < N; ++i) {
nlen += dx[i] * s[i] * dx[i] * s[i];
}
nlen = sqrt(nlen);
for (i = 0; i < N; ++i) {
if (fabs(x[i]) > 1.0 / fabs(dx[i])) {
den = fabs(x[i]);
}
else {
den = 1.0 / fabs(dx[i]);
}
rcheck[i] = s[i] / den;
}
rell = array_max_abs(rcheck, N);
stepmin = ftol / rell;
dginit = 0.0;
for (j = 0; j < N; ++j) {
dginit += g[j] * s[j];
}
if (dginit >= 0.0) {
return info;
}
brackt = 0;
stage1 = 1;
finit = *f;
nfev = 0;
dgtest = ftol*dginit;
width = maxstep - stepmin;
width1 = width / 0.5;
for (j = 0; j < N; ++j) {
wa[j] = x[j];
}
/* The variables stx, fx, dgx contain the values of the step,
function, and directional derivative at the best step.
The variables sty, fy, dgy contain the value of the step,
function, and derivative at the other endpoint of
the interval of uncertainty.
The variables stp, f, dg contain the values of the step,
function, and derivative at the current step.
*/
stx = 0.0;
fx = finit;
dgx = dginit;
sty = 0.0;
fy = finit;
dgy = dginit;
// Iteration
while (info == 0) {
if (brackt == 1) {
stmin = pmin(stx, sty);
stmax = pmax(stx, sty);
} else {
stmin = stx;
stmax = *stp + xtrapf*(*stp - stx);
}
*stp = pmax(*stp, stepmin);
*stp = pmin(*stp, maxstep);
if ((brackt == 1 && (*stp <= stmin || *stp >= stmax)) || nfev >= siter - 1 || infoc == 0 || (brackt == 1 && (stmax - stmin) <= xtol*stmax)) {
*stp = stx;
}
for (j = 0; j < N; ++j) {
x[j] = wa[j] + *stp * s[j];
}
*f = FUNCPT_EVAL(funcpt,x, N);
if (*f >= DBL_MAX || *f <= -DBL_MAX) {
printf("Program Exiting as the function value exceeds the maximum double value");
free(rcheck);
free(wa);
return 15;
}
if (*f != *f) {
printf("Program Exiting as the function returns NaN");
free(rcheck);
free(wa);
return 15;
}
grad_cd(funcpt,funcgrad, x, N, dx, eps2,g);
nfev++;
dg = 0.0;
for (j = 0; j < N; ++j) {
dg = dg + g[j]*s[j];
}
ftest1 = finit + *stp * dgtest;
// Test for convergence.
if ((brackt == 1 && (*stp <= stmin || *stp >= stmax)) || infoc == 0) {
info = 6;
}
if (*stp == maxstep && *f <= ftest1 && dg <= dgtest) {
info = 5;
}
if (*stp == stepmin && (*f > ftest1 || dg >= dgtest)) {
info = 4;
}
if (nfev >= siter) {
info = 3;
}
if (brackt == 1 && ((stmax - stmin) <= xtol*stmax)) {
info = 2;
}
if (*f <= ftest1 && fabs(dg) <= gtol*(-dginit)) {
info = 1;
}
if (stage1 == 1 && *f <= ftest1 && dg >= pmin(ftol, gtol)*dginit) {
stage1 = 0;
}
/*
A modified function is used to predict the step only if
we have not obtained a step for which the modified
function has a nonpositive function value and nonnegative
derivative, and if a lower function value has been
obtained but the decrease is not sufficient.
*/
if (stage1 == 1 && *f <= fx && *f > ftest1) {
fm = *f - *stp*dgtest;
fxm = fx - stx*dgtest;
fym = fy - sty*dgtest;
dgm = dg - dgtest;
dgxm = dgx - dgtest;
dgym = dgy - dgtest;
infoc = cstep(&stx, &fxm, &dgxm, &sty, &fym, &dgym, stp, &fm, &dgm, &brackt, stmin, stmax);
fx = fxm + stx*dgtest;
fy = fym + sty*dgtest;
dgx = dgxm + dgtest;
dgy = dgym + dgtest;
} else {
infoc = cstep(&stx, &fx, &dgx, &sty, &fy, &dgy, stp, f, &dg, &brackt, stmin, stmax);
}
if (brackt == 1) {
if (fabs(sty - stx) >= p66*width1) {
*stp = stx + p5*(sty - stx);
}
width1 = width;
width = fabs(sty - stx);
}
}
free(rcheck);
free(wa);
return info;
}
void qrfac(double *A, int M, int N, int lda, int pivot, int *ipvt, int lipvt,double *rdiag, double *acnorm,double eps) {
int i,j,jp1,k,kmax,minmn,t;
double ajnorm,epsmch,one,p05,sum,temp,zero,temp2,pmaxval;
double *AT,*wa,*wa2;
/*
* This routine is a C translation of Fortran Code by
* argonne national laboratory. minpack project. march 1980.
burton s. garbow, kenneth e. hillstrom, jorge j. more
*
* M is a positive integer input variable set to the number
of rows of a.
N is a positive integer input variable set to the number
of columns of a.
A is an M by N array. on input a contains the matrix for
which the qr factorization is to be computed. on output
the strict upper trapezoidal part of a contains the strict
upper trapezoidal part of r, and the lower trapezoidal
part of a contains a factored form of q (the non-trivial
elements of the u vectors described above).
lda is a positive integer input variable not less than m
which specifies the leading dimension of the array a.
pivot is an integer input variable. if pivot is set to 1,
then column pivoting is enforced. if pivot is set to 0,
then no column pivoting is done.
ipvt is an integer output array of length lipvt. ipvt
defines the permutation matrix p such that a*p = q*r.
column j of p is column ipvt(j) of the identity matrix.
if pivot is false, ipvt is not referenced.
lipvt is a positive integer input variable. if pivot is false,
then lipvt may be as small as 1. if pivot is true, then
lipvt must be at least n.
rdiag is an output array of length N which contains the
diagonal elements of r.
acnorm is an output array of length N which contains the
norms of the corresponding columns of the input matrix a.
if this information is not needed, then acnorm can coincide
with rdiag.
*
*/
if (pivot != 0 && pivot != 1) {
printf("Pivot only takes binary values 0 and 1 \n");
exit(-1);
}
AT = (double*) malloc(sizeof(double) *N*M);
wa = (double*) malloc(sizeof(double) *N);
wa2 = (double*) malloc(sizeof(double) *M);
one = 1.0;
zero = 0.0;
p05 = 5.0e-02;
epsmch = eps;
mtranspose(A,M,N,AT);// AT is size NXM
//compute the initial column norms and initialize several arrays.
for(j = 0; j < N; ++j) {
acnorm[j] = enorm(AT+j*M,M);
rdiag[j] = acnorm[j];
wa[j] = rdiag[j];
if (pivot == 1) {
ipvt[j] = j;
}
}
//reduce a to r with householder transformations.
if (M < N) {
minmn = M;
} else {
minmn = N;
}
for (j = 0; j < minmn; ++j) {
if (pivot == 1) {
//bring the column of largest norm into the pivot position.
kmax = j;
for(k = j; k < N; ++k) {
if (rdiag[k] > rdiag[kmax]) {
kmax = k;
}
}
if (kmax != j) {
for(i = 0; i < M; ++i) {
t = i * N;
temp = A[t+j];
A[t+j] = A[t+kmax];
A[t+kmax] = temp;
}
rdiag[kmax] = rdiag[j];
wa[kmax] = wa[j];
k = ipvt[j];
ipvt[j] = ipvt[kmax];
ipvt[kmax] = k;
}
}
// compute the householder transformation to reduce the
// j-th column of a to a multiple of the j-th unit vector.
t = j * N + j;
for(i = 0; i < M-j; ++i) {
wa2[i] = A[t+i*N];
}
ajnorm = enorm(wa2,M-j);
if (ajnorm != zero) {
if (A[t] < zero) {
ajnorm = - ajnorm;
}
for(i = j; i < M; ++i) {
A[i*N+j] /= ajnorm;
}
A[t] += one;
// apply the transformation to the remaining columns
// and update the norms.
jp1 = j + 1; // Breakpoint
if (N >= jp1+1) {
for(k = jp1; k < N; ++k) {
sum = zero;
for(i = j; i < M; ++i) {
sum += (A[i*N+j] * A[i*N+k]);
}
temp = sum / A[t];
for(i = j; i < M; ++i) {
A[i*N+k] -= (temp * A[i*N+j]);
}
// Breakpoint 2
if (pivot == 1 && rdiag[k] != zero) {
temp = A[j*N+k] / rdiag[k];
pmaxval = pmax(zero, one - temp*temp);
rdiag[k] = rdiag[k]*sqrt(pmaxval);
temp2 = (p05*(rdiag[k]/wa[k]));
temp2 = temp2 * temp2;
if (temp2 <= epsmch) {
for(i = 0; i < M-j-1; ++i) {
wa2[i] = A[jp1*N+k+i*N];
}
rdiag[k] = enorm(wa2,M-j-1);
wa[k] = rdiag[k];
}
}
}
}
}
rdiag[j] = -ajnorm;
}
free(AT);
free(wa);
free(wa2);
}
void lmpar(double *r,int ldr,int N,int *ipvt,double *diag,double *qtb,double delta,double *par,double *x,double *sdiag) {
int i,iter,j,jm1,jp1,k,l,nsing;
double dxnorm,dwarf,fp,gnorm,parc,parl,paru,p1,p001,sum,temp,zero;
double *wa1,*wa2;
/*
* This routine is a C translation of Fortran Code by
* argonne national laboratory. minpack project. march 1980.
burton s. garbow, kenneth e. hillstrom, jorge j. more
* N is a positive integer input variable set to the order of r.
r is an N by N array. on input the full upper triangle
must contain the full upper triangle of the matrix r.
on output the full upper triangle is unaltered, and the
strict lower triangle contains the strict upper triangle
(transposed) of the upper triangular matrix s.
ldr is a positive integer input variable not less than n
which specifies the leading dimension of the array r.
ipvt is an integer input array of length N which defines the
permutation matrix p such that a*p = q*r. column j of p
is column ipvt(j) of the identity matrix.
diag is an input array of length N which must contain the
diagonal elements of the matrix d.
qtb is an input array of length N which must contain the first
N elements of the vector (q transpose)*b.
delta is a positive input variable which specifies an upper
bound on the euclidean norm of d*x.
par is a nonnegative variable. on input par contains an
initial estimate of the levenberg-marquardt parameter.
on output par contains the final estimate.
x is an output array of length N which contains the least
squares solution of the system a*x = b, sqrt(par)*d*x = 0,
for the output par.
sdiag is an output array of length N which contains the
diagonal elements of the upper triangular matrix s.
*/
wa1 = (double*) malloc(sizeof(double) *N);
wa2 = (double*) malloc(sizeof(double) *N);
p1 = 1.0e-01;
p001 = 1.0e-03;
zero = 0.0;
dwarf = 2.22507385852e-308;
// compute and store in x the gauss-newton direction. if the
// jacobian is rank-deficient, obtain a least squares solution.
nsing = N;
for(j = 1; j <= N; ++j) {
wa1[j-1] = qtb[j-1];
if (r[(j-1)*N+j-1] == zero && nsing == N) {
nsing = j - 1;
}
if (nsing < N) {
wa1[j-1] = zero;
}
}
if (nsing >= 1) {//50
for(k = 1; k <= nsing; ++k) {
j = nsing - k + 1;
wa1[j-1] = wa1[j-1]/r[(j-1)*N+j-1];
temp = wa1[j-1];
jm1 = j - 1;
if (jm1 >= 1) {
for(i = 1; i <= jm1; ++i) {
wa1[i-1] = wa1[i-1] - r[(i-1)*N+j-1]*temp;
}
}
}
}//50
for (j = 0; j < N; ++j) {
l = ipvt[j];
x[l] = wa1[j];
}
// initialize the iteration counter.
// evaluate the function at the origin, and test
// for acceptance of the gauss-newton direction.
iter = 0;
for(j = 0; j < N; ++j) {
wa2[j] = diag[j]*x[j];
}
dxnorm = enorm(wa2,N);
fp = dxnorm - delta;
if (fp > p1*delta) {//220
// if the jacobian is not rank deficient, the newton
// step provides a lower bound, parl, for the zero of
// the function. otherwise set this bound to zero.
parl = zero;
if (nsing >= N) { //120 nsing only takes values upto N
for(j = 0; j < N; ++j) {
l = ipvt[j];
wa1[j] = diag[l]*(wa2[l]/dxnorm);
}
for(j = 0; j < N; ++j) { //110
sum = zero;
jm1 = j - 1;
if (jm1 >= 0) {//100
for(i = 0; i <= jm1; ++i) { //check
sum = sum + r[i*N+j]*wa1[i];
}
}//100
wa1[j] = (wa1[j] - sum)/r[j*N+j];
}//110
temp = enorm(wa1,N);
parl = ((fp/delta)/temp)/temp;
}//120
// calculate an upper bound, paru, for the zero of the function.
for(j = 0; j < N; ++j) { //140
sum = zero;
for(i = 0; i <= j; ++i) { //check
sum = sum + r[i*N+j]*qtb[i];
}
l = ipvt[j];
wa1[j] = sum/diag[l];
}//140
gnorm = enorm(wa1,N);
paru = gnorm/delta;
if (paru == zero) {
paru = dwarf/pmin(delta,p1);
}
// if the input par lies outside of the interval (parl,paru),
// set par to the closer endpoint.
*par = pmax(*par,parl);
*par = pmin(*par,paru);
if (*par == zero) {
*par = gnorm/dxnorm;
}
//Iteration begins
while(1) {
iter++;
// evaluate the function at the current value of par.
if (*par == zero) {
*par = pmax(dwarf,p001*paru);
}
temp = sqrt(*par);
for(j = 0; j < N; ++j) {
wa1[j] = temp*diag[j];
}
qrsolv(r,ldr,N,ipvt,wa1,qtb,x,sdiag);
for(j = 0; j < N; ++j) {
wa2[j] = diag[j]*x[j];
}
dxnorm = enorm(wa2,N);
temp = fp;
fp = dxnorm - delta;
// if the function is small enough, accept the current value
// of par. also test for the exceptional cases where parl
// is zero or the number of iterations has reached 10.
if (fabs(fp) <= p1*delta) {
break;
}
if (iter == 10) {
break;
}
if (parl == zero && fp <= temp && temp < zero) {
break;
}
// compute the newton correction.
for(j = 0; j < N; ++j) { //180
l = ipvt[j];
wa1[j] = diag[l]*(wa2[l]/dxnorm);
}//180
for(j = 0; j < N; ++j) { //210
wa1[j] = wa1[j]/sdiag[j];
temp = wa1[j];
jp1 = j + 1;
if (N >= jp1+1) {
for(i = jp1; i < N; ++i) {
wa1[i] = wa1[i] - r[i*N+j]*temp;
}
}
}//210
temp = enorm(wa1,N);
parc = ((fp/delta)/temp)/temp;
// depending on the sign of the function, update parl or paru.
if (fp > zero) {
parl = pmax(parl,*par);
}
if (fp < zero) {
paru = pmin(paru,*par);
}
// compute an improved estimate for par.
*par = pmax(parl,*par+parc);
}
}//220
if (iter == 0) {
*par = zero;
}
free(wa1);
free(wa2);
}
int EmbedPolyline(TopologicalGraph &G)
{if(G.nv() < 3 || G.ne() < 2)return -1;
int OldNumEdge = G.ne();
PSet1 propSave(G.Set());
G.MakeConnected();
if(!G.FindPlanarMap() )
{Tprintf("Not Planar Graph");
for(tedge e = G.ne(); e > OldNumEdge; e--) G.DeleteEdge(e);
return -1;
}
tbrin FirstBrin = 1;
bool MaxPlanar = (G.ne() != 3 * G.nv() - 6) ? false : true;
int len;
if(!FirstBrin && !MaxPlanar)
G.LongestFace(FirstBrin,len);
else if(FirstBrin == 0)
{FirstBrin = G.extbrin();FirstBrin = -G.acir[FirstBrin];}
if(!MaxPlanar && G.ZigZagTriangulate())return -2;
svector<short> ecolor(1, G.ne());
SchnyderDecomp(G,FirstBrin,ecolor);
GeometricGraph G0(G);
//Compute trees
tedge ee;
Prop<Tpoint> Ebend(G.Set(tedge()),PROP_DRAW_POINT_3);
svector<tbrin> FatherB(1,G.nv(),(tbrin)0); FatherB.SetName("FatherB");
svector<tbrin> FatherG(1,G.nv(),(tbrin)0); FatherG.SetName("FatherG");
svector<tbrin> FatherR(1,G.nv(),(tbrin)0); FatherR.SetName("FatherR");
svector<int> x(1,G.nv(),0), y(1,G.nv(),0);
x.clear(); y.clear();
compute_parents(G0, Blue, -FirstBrin, FatherB, ecolor);
compute_parents(G0, Red, FirstBrin, FatherR, ecolor);
compute_parents(G0, Green, -G0.acir[FirstBrin], FatherG, ecolor);
// Compute the number of leaves of each tree
int nb_leavesB, nb_leavesR, nb_leavesG;
nb_leavesB = nb_leaves(G0, Blue, -FirstBrin, ecolor);
nb_leavesR = nb_leaves(G0, Red, FirstBrin, ecolor);
nb_leavesG = nb_leaves(G0, Green, -G0.acir[FirstBrin], ecolor);
// Compute the coordinates using the tree with the minimum number of leaves
ForAllEdges(ee, G) Ebend[ee] = Tpoint(-1,-1);
if (nb_leavesB <= nb_leavesR && nb_leavesB <= nb_leavesG)
compute_coords(G0,Red,Blue,-FirstBrin,FatherB,FatherR,FatherG,ecolor,x,y,Ebend);
else if (nb_leavesR <= nb_leavesG)
compute_coords(G0,Green,Red,G0.acir[FirstBrin],FatherR,FatherG,FatherB,ecolor,x,y,Ebend);
else
compute_coords(G0,Blue,Green,G0.acir[-G0.acir[FirstBrin]],FatherG,FatherB,FatherR,ecolor,x,y,Ebend);
// computes extremities of vertices
Prop<Tpoint> Epoint1(G.Set(tedge()),PROP_DRAW_POINT_1);
Prop<Tpoint> Epoint2(G.Set(tedge()),PROP_DRAW_POINT_2);
Prop<Tpoint> Vcoord(G.Set(tvertex()),PROP_DRAW_POINT_1);
G.Set() = propSave;
Prop1<Tpoint> pmin(G.Set(),PROP_POINT_MIN);
Prop1<Tpoint> pmax(G.Set(),PROP_POINT_MAX);
tvertex vv;
pmin() = Tpoint(0,0);
pmax() = Tpoint(0,0);
ForAllVertices(vv, G0)
{Vcoord[vv] = Tpoint(x[vv], y[vv]);
if (Vcoord[vv].x() > pmax().x())
pmax().x() = Vcoord[vv].x();
if (Vcoord[vv].y() > pmax().y())
pmax().y() = Vcoord[vv].y();
}
int lmder(custom_funcmult *funcmult, custom_jacobian *jacobian, double *x, int M, int N,
double *fvec,double *fjac,int ldfjac,int maxfev,double *diag,int mode,double factor,int nprint,
double eps,double ftol,double gtol,double xtol,int *nfev,int *njev,int *ipvt, double *qtf) {
int info;
int i,j,l,iter;
double actred,delta,dirder,epsmch,fnorm,fnorm1,gnorm,one,par,pnorm,prered,p1,p5,p25,p75,p0001,ratio,
sum,temp,temp1,temp2,xnorm,zero;
double *wa1,*wa2,*wa3,*wa4;
/*
* * This routine is a C translation of Fortran Code by
* argonne national laboratory. minpack project. march 1980.
burton s. garbow, kenneth e. hillstrom, jorge j. more
* M is a positive integer input variable set to the number
c of functions.
c
c N is a positive integer input variable set to the number
c of variables. N must not exceed M.
c
c x is an array of length N. on input x must contain
c an initial estimate of the solution vector. on output x
c contains the final estimate of the solution vector.
c
c fvec is an output array of length M which contains
c the functions evaluated at the output x.
c
c fjac is an output M by N array. the upper N by N submatrix
c of fjac contains an upper triangular matrix r with
c diagonal elements of nonincreasing magnitude such that
c
c t t t
c p *(jac *jac)*p = r *r,
c
c where p is a permutation matrix and jac is the final
c calculated jacobian. column j of p is column ipvt(j)
c (see below) of the identity matrix. the lower trapezoidal
c part of fjac contains information generated during
c the computation of r.
c
c ldfjac is a positive integer input variable not less than M
c which specifies the leading dimension of the array fjac.
c
c ftol is a nonnegative input variable. termination
c occurs when both the actual and predicted relative
c reductions in the sum of squares are at most ftol.
c therefore, ftol measures the relative error desired
c in the sum of squares.
c
c xtol is a nonnegative input variable. termination
c occurs when the relative error between two consecutive
c iterates is at most xtol. therefore, xtol measures the
c relative error desired in the approximate solution.
c
c gtol is a nonnegative input variable. termination
c occurs when the cosine of the angle between fvec and
c any column of the jacobian is at most gtol in absolute
c value. therefore, gtol measures the orthogonality
c desired between the function vector and the columns
c of the jacobian.
c
c maxfev is a positive integer input variable. termination
c occurs when the number of calls to fcn with iflag = 1
c has reached maxfev.
c
c diag is an array of length N. if mode = 1 (see
c below), diag is internally set. if mode = 2, diag
c must contain positive entries that serve as
c multiplicative scale factors for the variables.
c
c mode is an integer input variable. if mode = 1, the
c variables will be scaled internally. if mode = 2,
c the scaling is specified by the input diag. other
c values of mode are equivalent to mode = 1.
c
c factor is a positive input variable used in determining the
c initial step bound. this bound is set to the product of
c factor and the euclidean norm of diag*x if nonzero, or else
c to factor itself. in most cases factor should lie in the
c interval (.1,100.).100. is a generally recommended value.
c
c nprint is an integer input variable that enables controlled
c printing of iterates if it is positive. in this case,
c fcn is called with iflag = 0 at the beginning of the first
c iteration and every nprint iterations thereafter and
c immediately prior to return, with x, fvec, and fjac
c available for printing. fvec and fjac should not be
c altered. if nprint is not positive, no special calls
c of fcn with iflag = 0 are made.
c
c info is an integer output variable. if the user has
c terminated execution, info is set to the (negative)
c value of iflag. see description of fcn. otherwise,
c info is set as follows.
c
c info = 0 improper input parameters.
c
c info = 1 both actual and predicted relative reductions
c in the sum of squares are at most ftol.
c
c info = 2 relative error between two consecutive iterates
c is at most xtol.
c
c info = 3 conditions for info = 1 and info = 2 both hold.
c
c info = 4 the cosine of the angle between fvec and any
c column of the jacobian is at most gtol in
c absolute value.
c
c info = 5 number of calls to fcn with iflag = 1 has
c reached maxfev.
c
c info = 6 ftol is too small. no further reduction in
c the sum of squares is possible.
c
c info = 7 xtol is too small. no further improvement in
c the approximate solution x is possible.
c
c info = 8 gtol is too small. fvec is orthogonal to the
c columns of the jacobian to machine precision.
c
c nfev is an integer output variable set to the number of
c calls to fcn with iflag = 1.
c
c njev is an integer output variable set to the number of
c calls to fcn with iflag = 2.
c
c ipvt is an integer output array of length N. ipvt
c defines a permutation matrix p such that jac*p = q*r,
c where jac is the final calculated jacobian, q is
c orthogonal (not stored), and r is upper triangular
c with diagonal elements of nonincreasing magnitude.
c column j of p is column ipvt(j) of the identity matrix.
c
c qtf is an output array of length N which contains
c the first n elements of the vector (q transpose)*fvec.
*/
wa1 = (double*) malloc(sizeof(double) *N);
wa2 = (double*) malloc(sizeof(double) *N);
wa3 = (double*) malloc(sizeof(double) *N);
wa4 = (double*) malloc(sizeof(double) *M);
one = 1.0;
zero = 0.0;
p1 = 1.0e-1;
p5 = 5.0e-1;
p25 = 2.5e-1;
p75 = 7.5e-1;
p0001 = 1.0e-4;
epsmch = eps;
info = 0;
*nfev = 0;
*njev = 0;
if (N <= 0 || M < N || ldfjac < M || ftol < zero || xtol < zero || gtol < zero || maxfev <= 0 || factor <= zero) {
return info;
}
if (mode == 2) {
for(j = 0; j < N; ++j) {
if (diag[j] <= 0.0) {
return info;
}
}
}
// evaluate the function at the starting point
// and calculate its norm.
FUNCMULT_EVAL(funcmult,x,M,N,fvec);
*nfev= 1;
fnorm = enorm(fvec,M);
// initialize levenberg-marquardt parameter and iteration counter.
par = zero;
iter = 1;
ratio = zero;
// beginning of the outer loop.
while(1) {
// calculate the jacobian matrix.
ratio = zero;
JACOBIAN_EVAL(jacobian,x,M,N,fjac);
*njev = *njev +1;
// compute the qr factorization of the jacobian.
qrfac(fjac,M,N,ldfjac,1,ipvt,N,wa1,wa2,eps);
// on the first iteration and if mode is 1, scale according
// to the norms of the columns of the initial jacobian.
if (iter == 1) {//80
if (mode != 2) {//60
for(j = 0; j < N; ++j) {
diag[j] = wa2[j];
if (wa2[j] == zero) {
diag[j] = one;
}
}
}//60
// on the first iteration, calculate the norm of the scaled x
// and initialize the step bound delta.
for(j = 0; j < N; ++j) {
wa3[j] = diag[j]*x[j];
}
xnorm = enorm(wa3,N);
delta = factor*xnorm;
if (delta == zero) {
delta = factor;
}
}//80
// form (q transpose)*fvec and store the first n components in
// qtf.
for(i = 0; i < M; ++i) {
wa4[i] = fvec[i];
}
for(j = 0; j < N; ++j) { //130
if (fjac[j*N+j] != zero) {//120
sum = zero;
for(i = j; i < M; ++i) { //100
sum = sum + fjac[i*N+j]*wa4[i];
}//100
temp = -sum/fjac[j*N+j];
for(i = j; i < M; ++i) { //110
wa4[i] = wa4[i] + fjac[i*N+j]*temp;
}//110
}//120
fjac[j*N+j] = wa1[j];
qtf[j] = wa4[j];
}//130
// compute the norm of the scaled gradient.
gnorm = zero;
if (fnorm != zero) {//170
for(j = 0; j < N; ++j) { //160
l = ipvt[j];
if (wa2[l] != zero) {//150
sum = zero;
for(i = 0; i <= j; ++i) { //140
sum = sum + fjac[i*N+j]*(qtf[i]/fnorm);
}//140
gnorm = pmax(gnorm,fabs(sum/wa2[l]));
}//150
}//160
}//170
// test for convergence of the gradient norm.
if (gnorm <= gtol) {
info = 4;
}
if (info != 0) {
break;
}
// rescale if necessary.
if (mode != 2) { //190
for(j = 0; j < N; ++j) {
diag[j] = pmax(diag[j],wa2[j]);
}
}//190
// beginning of the inner loop.
while(ratio < p0001) {
// determine the levenberg-marquardt parameter.
lmpar(fjac,ldfjac,N,ipvt,diag,qtf,delta,&par,wa1,wa2);
// store the direction p and x + p. calculate the norm of p.
for(j = 0; j < N; ++j) {
wa1[j] = -wa1[j];
wa2[j] = x[j] + wa1[j];
wa3[j] = diag[j]*wa1[j];
}
pnorm = enorm(wa3,N);
// on the first iteration, adjust the initial step bound.
if (iter == 1) {
delta = pmin(delta,pnorm);
}
// evaluate the function at x + p and calculate its norm.
FUNCMULT_EVAL(funcmult,wa2,M,N,wa4);
*nfev = *nfev + 1;
fnorm1 = enorm(wa4,M);
// compute the scaled actual reduction.
actred = -one;
if (p1*fnorm1 < fnorm) {
actred = one - (fnorm1/fnorm)*(fnorm1/fnorm);
}
// compute the scaled predicted reduction and
// the scaled directional derivative.
for(j = 0; j < N; ++j) {
wa3[j] = zero;
l = ipvt[j];
temp = wa1[l];
for(i = 0; i <= j; ++i) {
wa3[i] = wa3[i] + fjac[i*N+j]*temp;
}
}
temp1 = enorm(wa3,N);
temp1 = temp1/fnorm;
temp2 = (sqrt(par)*pnorm)/fnorm;
prered = temp1*temp1 + temp2*temp2/p5;
dirder = -(temp1*temp1 + temp2*temp2);
// compute the ratio of the actual to the predicted
// reduction.
ratio = zero;
if (prered != zero) {
ratio = actred/prered;
}
// update the step bound.
if (ratio <= p25) {//240
if (actred >= zero) {
temp = p5;
}
if (actred < zero) {
temp = p5*dirder/(dirder + p5*actred);
}
if (p1*fnorm1 >= fnorm || temp < p1) {
temp = p1;
}
delta = temp*pmin(delta,pnorm/p1);
par = par/temp;
} else if (par == zero || ratio >= p75) { //240 - 260
delta = pnorm/p5;
par = p5*par;
}//260
// test for successful iteration.
if (ratio >= p0001) {//290
// successful iteration. update x, fvec, and their norms.
for(j = 0; j < N; ++j) {
x[j] = wa2[j];
wa2[j] = diag[j]*x[j];
}
for(i = 0; i < M; ++i) {
fvec[i] = wa4[i];
}
xnorm = enorm(wa2,N);
fnorm = fnorm1;
iter = iter + 1;
}//290
// tests for convergence.
if ((fabs(actred) <= ftol) && (prered <= ftol) && (p5*ratio <= one)) {
info = 1;
}
if (delta <= xtol*xnorm) {
info = 2;
}
if ((fabs(actred) <= ftol) && (prered <= ftol) && (p5*ratio <= one) && (info == 2)) {
info = 3;
}
if (info != 0) {
break;
}
// tests for termination and stringent tolerances.
if (*nfev >= maxfev) {
info = 5;
}
if ((fabs(actred) <= epsmch) && (prered <= epsmch) && (p5*ratio <= one)) {
info = 6;
}
if (delta <= epsmch*xnorm) {
info = 7;
}
if (gnorm <= epsmch) {
info = 8;
}
if (info != 0) {
break;
}
}
if (info != 0) {
break;
}
}
free(wa1);
free(wa2);
free(wa3);
free(wa4);
return info;
}
void DrawBip2Pages(QPainter *p,pigalePaint *paint)
{GraphContainer GC = paint->GCP;
GeometricGraph G(paint->GCP);
Prop<int> h(G.Set(tvertex()),PROP_DRAW_INT_1);
Prop<int> h1(G.Set(tvertex()),PROP_DRAW_INT_2);
Prop<int> h2(G.Set(tvertex()),PROP_DRAW_INT_3);
Prop1<Tpoint> pmax(G.Set(),PROP_POINT_MAX);
Prop1<Tpoint> pmin(G.Set(),PROP_POINT_MIN);
QPen pn = p->pen();pn.setWidth(1);
QPoint ps,pt,ps2,pt2;
// horizontale
int yh = pmin().y()+ pmax().y()/2;
ps = QPoint(paint->to_x(pmin().x()),paint->to_y(yh));
pt = QPoint(paint->to_x(pmax().x()),paint->to_y(yh));
pn.setWidth(1);pn.setColor(color[Grey1]); p->setPen(pn);
p->drawLine(ps,pt);
pn.setColor(color[Black]);
pn.setWidth(1);p->setPen(pn);
// draw edges: 2 segments
double x1,x2,dh;
tvertex vmin,vmax;
for(tedge e = 1; e < G.ne();e++)
{tvertex v1 = G.vin[e];
tvertex v2 = G.vin[-e];
if(h[v1] <h[v2])
{vmin = v1;vmax = v2;}
else
{vmin = v2;vmax = v1;}
x1 = (double)Min(h[v1],h[v2]);
x2 = (double)Max(h[v1],h[v2]);
dh = Min((x2-x1)*paint->xscale,(x2-x1)*paint->yscale)/2;
QRect r = QRect(paint->to_x(x1),paint->to_y(yh)-dh/2,(int)((x2-x1)*paint->xscale),(int)dh);
if(G.vcolor[vmax] == Red)
p->drawArc(r,0,180*16);
else
p->drawArc(r,0,-180*16);
}
// Draw verticces
QBrush pb = p->brush();
pb.setStyle(Qt::SolidPattern);
pb.setColor(color[White]);
p->setBrush(pb);
int dy = Min(10,paint->height()/(pmax().y()+1)-2);
QFont font = QFont("sans",dy);
p->setFont(font);
pn.setWidth(1);
for(tvertex v = 1;v <= G.nv();v++)
{ps = QPoint(paint->to_x(h[v]),paint->to_y(yh));
QString t = getVertexLabel(GC,v);
QSize size = QFontMetrics(font).size(Qt::AlignCenter,t);
int dx =size.width() + 2; dy =size.height();// + 2;
if(t.length() == 0)dx = dy= 8;
QRect rect = QRect(ps.x()-dx/2 ,ps.y()-dy/2,dx,dy);
pn.setColor(color[G.vcolor[v]]);p->setPen(pn);
p->drawRect(rect);
pn.setColor(color[Black]);p->setPen(pn);
p->drawText(rect,Qt::AlignCenter,t);
}
}
int cstep(double *stx,double *fx,double *dx,double *sty,double *fy,double *dy,double *stp,double *fp,double *dp,int *brackt,
double stpmin,double stpmax) {
int info,bound;
double gamma, p, p66, q, r, s, sgnd, stpc, stpf, stpq, theta;
info = 0;
p66 = 0.66;
if ((*brackt == 1 && (*stp <= pmin(*stx, *sty) || *stp >= pmax(*stx, *sty))) || *dx*(*stp - *stx) >= 0.0 || stpmax < stpmin) {
return info;
}
sgnd = (*dp) * (*dx / fabs(*dx));
/*
First case. A higher function value. (fp > fx)
The minimum is bracketed. If the cubic step is closer
to stx than the quadratic step, the cubic step is taken,
else the average of the cubic and quadratic steps is taken.
*/
if (*fp > *fx) {
info = 1;
bound = 1;
theta = 3 * (*fx - *fp) / (*stp - *stx) + *dx + *dp;
s = pmax(fabs(theta), fabs(*dx));
s = pmax(s, fabs(*dp));
gamma = s*sqrt((theta / s)*(theta/s) - (*dx / s)*(*dp / s));
if (*stp < *stx) {
gamma = -gamma;
}
p = (gamma - *dx) + theta;
q = ((gamma - *dx) + gamma) + *dp;
r = p / q;
stpc = *stx + r*(*stp - *stx);
stpq = *stx + ((*dx / ((*fx - *fp) / (*stp - *stx) + *dx)) / 2)*(*stp - *stx);
if (fabs(stpc - *stx) < fabs(stpq - *stx)) {
stpf = stpc;
} else {
stpf = stpc + (stpq - stpc) / 2;
}
*brackt = 1;
} else if (sgnd < 0.0) {
/*
Second case. A lower function value and derivatives of
opposite sign. The minimum is bracketed. If the cubic
step is closer to stx than the quadratic (secant) step,
the cubic step is taken, else the quadratic step is taken.
*/
info = 2;
bound = 0;
theta = 3 * (*fx - *fp) / (*stp - *stx) + *dx + *dp;
s = pmax(fabs(theta), fabs(*dx));
s = pmax(s, fabs(*dp));
gamma = s*sqrt((theta / s)*(theta / s) - (*dx / s)*(*dp / s));
if (*stp > *stx) {
gamma = -gamma;
}
p = (gamma - *dp) + theta;
q = ((gamma - *dp) + gamma) + *dx;
r = p / q;
stpc = *stp + r*(*stx - *stp);
stpq = *stp + (*dp / (*dp - *dx))*(*stx - *stp);
if (fabs(stpc - *stp) > fabs(stpq - *stp)) {
stpf = stpc;
} else {
stpf = stpq;
}
*brackt = 1;
}
else if (fabs(*dp) < fabs(*dx)) {
/*
Third case. A lower function value, derivatives of the
same sign, and the magnitude of the derivative decreases.
The cubic step is only used if the cubic tends to infinity
in the direction of the step or if the minimum of the cubic
is beyond stp. Otherwise the cubic step is defined to be
either stpmin or stpmax. The quadratic (secant) step is also
computed and if the minimum is bracketed then the the step
closest to stx is taken, else the step farthest away is taken.
*/
info = 3;
bound = 1;
theta = 3 * (*fx - *fp) / (*stp - *stx) + *dx + *dp;
s = pmax(fabs(theta), fabs(*dx));
s = pmax(s, fabs(*dp));
/*
The case gamma = 0 only arises if the cubic does not tend
to infinity in the direction of the step.
*/
gamma = s*sqrt(pmax(0., (theta / s)*(theta / s) - (*dx / s)*(*dp / s)));
if (*stp > *stx) {
gamma = -gamma;
}
p = (gamma - *dp) + theta;
q = (gamma + (*dx - *dp)) + gamma;
r = p / q;
if (r < 0.0 && gamma != 0.0) {
stpc = *stp + r*(*stx - *stp);
} else if (*stp > *stx) {
stpc = stpmax;
} else {
stpc = stpmin;
}
stpq = *stp + (*dp / (*dp - *dx))*(*stx - *stp);
if (*brackt == 1) {
if (fabs(*stp - stpc) < fabs(*stp - stpq)) {
stpf = stpc;
} else {
stpf = stpq;
}
} else {
if (fabs(*stp - stpc) > fabs(*stp - stpq)) {
stpf = stpc;
} else {
stpf = stpq;
}
}
}
else {
/*
Fourth case. A lower function value, derivatives of the
same sign, and the magnitude of the derivative does
not decrease. If the minimum is not bracketed, the step
is either stpmin or stpmax, else the cubic step is taken.
*/
info = 4;
bound = 0;
if (*brackt == 1) {
theta = 3 * (*fp - *fy) / (*sty - *stp) + *dy + *dp;
s = pmax(fabs(theta), fabs(*dy));
s = pmax(s,fabs(*dp));
gamma = s*sqrt((theta / s)*(theta / s) - (*dy / s)*(*dp / s));
if (*stp > *sty) {
gamma = -gamma;
}
p = (gamma - *dp) + theta;
q = ((gamma - *dp) + gamma) + *dy;
r = p / q;
stpc = *stp + r*(*sty - *stp);
stpf = stpc;
}
else if (*stp > *stx) {
stpf = stpmax;
}
else {
stpf = stpmin;
}
}
/*
Update the interval of uncertainty. This update does not
depend on the new step or the case analysis above.
*/
if (*fp > *fx) {
*sty = *stp;
*fy = *fp;
*dy = *dp;
} else {
if (sgnd < 0.0) {
*sty = *stx;
*fy = *fx;
*dy = *dx;
}
*stx = *stp;
*fx = *fp;
*dx = *dp;
}
/*
Compute the new step and safeguard it.
*/
stpf = pmin(stpmax, stpf);
stpf = pmax(stpmin, stpf);
*stp = stpf;
if (*brackt == 1 && bound == 1) {
if (*sty > *stx) {
*stp = pmin(*stx + p66*(*sty - *stx), *stp);
} else {
*stp = pmax(*stx + p66*(*sty - *stx), *stp);
}
}
return info;
}
void DrawBipContact(QPainter *p,pigalePaint *paint)
{GraphContainer GC = paint->GCP;
GeometricGraph G(paint->GCP);
Prop<int> h(G.Set(tvertex()),PROP_DRAW_INT_1);
Prop<int> h1(G.Set(tvertex()),PROP_DRAW_INT_2);
Prop<int> h2(G.Set(tvertex()),PROP_DRAW_INT_3);
Prop1<Tpoint> pmax(G.Set(),PROP_POINT_MAX);
Prop1<Tpoint> pmin(G.Set(),PROP_POINT_MIN);
QPen pn = p->pen();pn.setWidth(1);
QBrush pb = p->brush();
pb.setStyle(Qt::SolidPattern);
QPoint ps,pt,ps2,pt2;
// Drawing the diagonal
ps = QPoint(paint->to_x(pmin().x()),paint->to_y(pmin().y()));
pt = QPoint(paint->to_x(pmax().x()),paint->to_y(pmax().y()));
pn.setColor(color[Grey1]); p->setPen(pn);
p->drawLine(ps,pt);
// Drawing the vertices: horizontal and vertical segments
for(tvertex v = 1;v <= G.nv();v++)
{double delta = (h1[v] < h[v])?.45:-.45;
pn.setColor(color[G.vcolor[v]]);
pn.setWidth(1);p->setPen(pn);
if(G.vcolor[v] == Red)//horizontales
{ps = QPoint(paint->to_x(h1[v]),paint->to_y(h[v]));
pt = QPoint(paint->to_x(h2[v]),paint->to_y(h[v]));
if(h2[v] > h[v] && h1[v] > h[v])
{ps2 = QPoint(paint->to_x(h[v]),paint->to_y(h[v]));
p->drawLine(ps2,pt);
}
else if(h2[v] < h[v] && h1[v] < h[v])
{pt2 = QPoint(paint->to_x(h[v]),paint->to_y(h[v]));
p->drawLine(ps,pt2);
}
if(h1[v] == h2[v])// isthme
{ps = QPoint(paint->to_x(h1[v]),paint->to_y(h[v]));
pt = QPoint(paint->to_x(h2[v]+delta),paint->to_y(h[v]));
}
pn.setWidth(2);p->setPen(pn);
p->drawLine(ps,pt);
}
else // verticales
{ps = QPoint(paint->to_x(h[v]),paint->to_y(h1[v]));
pt = QPoint(paint->to_x(h[v]),paint->to_y(h2[v]));
if(h2[v] > h[v] && h1[v] > h[v])
{ps2 = QPoint(paint->to_x(h[v]),paint->to_y(h[v]));
p->drawLine(ps2,pt);
}
else if(h2[v] < h[v] && h1[v] < h[v])
{pt2 = QPoint(paint->to_x(h[v]),paint->to_y(h[v]));
p->drawLine(ps,pt2);
}
if(h1[v] == h2[v])// isthme
{ps = QPoint(paint->to_x(h[v]),paint->to_y(h1[v]));
pt = QPoint(paint->to_x(h[v]),paint->to_y(h2[v]+delta));
}
pn.setWidth(2);p->setPen(pn);
p->drawLine(ps,pt);
}
}
//drawing labels
int dy = Min(12,paint->height()/(pmax().y()+1)-2);
QFont font = QFont("sans",dy);
p->setFont(font);
pb.setColor(color[White]);
p->setBrush(pb);
pn.setWidth(1);
for(tvertex v = 1;v <= G.nv();v++) //trace des labels
{double delta = (h1[v] < h[v])?.9:-.9;
if(G.vcolor[v] == Red)
{if(h1[v] != h2[v])
{ps = QPoint(paint->to_x(h1[v]),paint->to_y(h[v]));
pt = QPoint(paint->to_x(h2[v]),paint->to_y(h[v]));
}
else
{ps = QPoint(paint->to_x(h1[v]),paint->to_y(h[v]));
pt = QPoint(paint->to_x(h2[v]+delta),paint->to_y(h[v]));
}
}
else
{if(h1[v] != h2[v])
{ps = QPoint(paint->to_x(h[v]),paint->to_y(h1[v]));
pt = QPoint(paint->to_x(h[v]),paint->to_y(h2[v]));
}
else
{ps = QPoint(paint->to_x(h[v]),paint->to_y(h1[v]));
pt = QPoint(paint->to_x(h[v]),paint->to_y(h2[v]+delta));
}
}
QString t = getVertexLabel(GC,v);
QSize size = QFontMetrics(font).size(Qt::AlignCenter,t);
int dx =size.width() + 2; dy =size.height() + 2;
if(t.length() == 0)dx = dy= 8;
QRect rect = QRect((ps.x() + pt.x() - dx)/2,(ps.y() + pt.y() - dy)/2,dx,dy);
pn.setColor(color[G.vcolor[v]]);p->setPen(pn);
p->drawRect(rect);
pn.setColor(color[Black]);p->setPen(pn);
p->drawText(rect,Qt::AlignCenter,t);
}
}
void TransformableGraphicsGuide::update()
{
if (isVisible())
{
FigureEditor::EditMode mode = editor->mode();
bool scaleMode( mode == FigureEditor::Scale );
topRightRect.setVisible(scaleMode);
topLeftRect.setVisible(scaleMode);
bottomRightRect.setVisible(scaleMode);
bottomLeftRect.setVisible(scaleMode);
if (scaleMode)
{
QPointF cen;
QPolygonF poly;
if (editor->hasSelection())
{
QGraphicsPolygonItem* item = editor->selection();
if (item == 0)
return;
cen = mapFromScene(editor->selectionTransformPos());
poly = mapFromScene(item->polygon());
}
else
{
QGraphicsPolygonItem* item = dynamic_cast<QGraphicsPolygonItem*>(parentItem());
cen = editor->triangleTransformPos();
poly = item->polygon();
}
QRectF f( poly.boundingRect() );
qreal xmax = qMax(qAbs(f.left() - cen.x()), qAbs(f.right() - cen.x()));
qreal ymax = qMax(qAbs(f.top() - cen.y()), qAbs(f.bottom() - cen.y()));
QPointF pmax(xmax, ymax);
QRectF r(pmax, -pmax);
r.moveCenter(cen);
outerRect = r;
QRectF l = parentItem()->mapRectFromScene(QRectF(QPointF(0.0, 0.0), QSizeF(10, 10)));
QPen pen( editor->guideColor() );
l.moveBottomLeft(r.topRight());
topRightRect.setPen(pen);
topRightRect.setRect(l);
l.moveBottomRight(r.topLeft());
topLeftRect.setPen(pen);
topLeftRect.setRect(l);
l.moveTopLeft(r.bottomRight());
bottomRightRect.setPen(pen);
bottomRightRect.setRect(l);
l.moveTopRight(r.bottomLeft());
bottomLeftRect.setPen(pen);
bottomLeftRect.setRect(l);
}
else if (mode == FigureEditor::Rotate)
{
QGraphicsPolygonItem* item;
QPointF cen;
QPolygonF poly;
if (editor->hasSelection())
{
item = editor->selection();
if (item == 0)
return;
cen = mapFromScene(editor->selectionTransformPos());
poly = mapFromScene(item->polygon());
}
else
{
item = dynamic_cast<QGraphicsPolygonItem*>(parentItem());
poly = item->polygon();
cen = editor->triangleTransformPos();
}
qreal rmax = 0.0;
foreach (QPointF p, poly)
{
QLineF l(p, cen);
qreal len(l.length());
if (len > rmax)
rmax = len;
}
qreal height = rmax * 2.0;
outerRect = QRectF(cen.x() - rmax, cen.y() - rmax, height, height);
}
bool srs_env_model::COctoMapPlugin::addCubeCB(
srs_env_model::AddCube::Request & req,
srs_env_model::AddCube::Response & res) {
PERROR( "Add cube to octomap: " << req.pose << " --- \n size: \n" << req.size );
// Debug - show cube position
//addCubeGizmo( req.pose, req.size );
// Test frame id
if (req.frame_id != m_mapParameters.frameId) {
// Transform pose
geometry_msgs::PoseStamped ps, psout;
ps.header.frame_id = req.frame_id;
ps.header.stamp = m_mapParameters.currentTime;
ps.pose = req.pose;
m_tfListener.transformPose(m_mapParameters.frameId, ps, psout);
req.pose = psout.pose;
// Transform size
geometry_msgs::PointStamped vs, vsout;
vs.header.frame_id = req.frame_id;
vs.header.stamp = m_mapParameters.currentTime;
vs.point = req.size;
m_tfListener.transformPoint(m_mapParameters.frameId, vs, vsout);
req.size = vsout.point;
// PERROR( "Transformed cube from octomap: " << req.pose << " --- " << req.size );
}
PERROR( "Computing sizes..." );
// Compute minimal and maximal value
octomap::point3d pmin, pmax;
pmin(0) = req.pose.position.x - req.size.x * 0.5;
pmin(1) = req.pose.position.y - req.size.y * 0.5;
pmin(2) = req.pose.position.z - req.size.z * 0.5;
pmax(0) = req.pose.position.x + req.size.x * 0.5;
pmax(1) = req.pose.position.y + req.size.y * 0.5;
pmax(2) = req.pose.position.z + req.size.z * 0.5;
PERROR( "Sizes computed. Computing steps ");
float diff[3];
unsigned int steps[3];
for (int i=0;i<3;++i)
{
diff[i] = pmax(i) - pmin(i);
steps[i] = floor(diff[i] / m_mapParameters.resolution);
std::cerr << "bbx " << i << " size: " << diff[i] << " " << steps[i] << " steps\n";
}
PERROR("Rendering... Resolution: " << m_mapParameters.resolution );
long counter(0);
octomap::point3d p = pmin;
for (unsigned int x = 0; x < steps[0]; ++x)
{
p.x() += m_mapParameters.resolution;
p.y() = pmin.y();
for (unsigned int y = 0; y < steps[1]; ++y)
{
p.y() += m_mapParameters.resolution;
p.z() = pmin.z();
for (unsigned int z = 0; z < steps[2]; ++z)
{
// std::cout << "querying p=" << p << std::endl;
p.z() += m_mapParameters.resolution;
++counter;
m_data->getTree().updateNode(p, true, true);
}
}
}
PERROR( "Added. Changed nodes: " << counter );
return true;
}
