/*Returns nearest edge to (x,y) by returning prior vertex
*/
public cVertex GetEdge(float x, float y)
{
cVertex vnear = null, vtemp = head;
float mindist = 0, dist = -1.0f;
//int k;
cPointi p = new cPointi();
// input query point
p.X = x;
p.Y = y;
if (vtemp == null)
{
return(vnear);
}
do
{
dist = p.DistEdgePoint(vtemp.Point, vtemp.NextVertex.Point, p);
vtemp.Point.PrintPoint();
if (vnear == null || dist < mindist)
{
mindist = dist;
vnear = vtemp;
}
vtemp = vtemp.NextVertex;
} while (vtemp != head);
return(vnear);
}
}//end TwoCircles00
/*Method used for cretaing the "extra-points" that will be displayed
* on the screen after the arm is straightened. Called from DrawPoints */
public cVertexList MakePoints(int lo, int hi1, int hi2, cVertex first, cVertex last, cVertexList listcopy)
{
float xaux; //auxiliary variable for storin the info
float lenaux = 0; //auxiliary variable for storing the Length of the
//current link
cPointi v1 = new cPointi(0, 0); //aux variable for computing the values
//of the new points
float sum = 0; //the sum of the previous link Lengths
for (i = lo; i < hi1; i++)
{
lenaux += linklenback[i];
}
sum = 0;
for (i = lo; i < hi2; i++)
{
sum += linklenback[i];
xaux = sum / (float)lenaux;
v1.X = (int)(.5 + (1 - xaux) * first.Point.X + xaux * last.Point.X);
v1.Y = (int)(.5 + (1 - xaux) * first.Point.Y + xaux * last.Point.Y);
listcopy.SetVertex(v1.X, v1.Y);
}//end for
return(listcopy);
}
private cPointi AddVec(cPointi a, cPointi b)
{
cPointi c = new cPointi();
c.X = a.X + b.X;
c.Y = a.Y + b.Y;
return(c);
}
private cPointi SubVec(cPointi a, cPointi b)
{
cPointi c = new cPointi();
c.X = a.X - b.X;
c.Y = a.Y - b.Y;
return(c);
}
}//end Solve3
public bool Solve2(float L1, float L2, cPointi target, cPointd J)
{
cPointi c1 = new cPointi(list.head.Point.X, list.head.Point.Y); // center of circle 1
int nsoln; // # of solns: 0,1,2,3(infinite)
nsoln = TwoCircles(c1, L1, target, L2, J);
return(nsoln != 0);
}// end Solve2
public float Length(cPointi point1, cPointi point2)
/*Computes the Length of the link between two points
* Used for the Solven (and the subsequent) methods.*/
{
return(Convert.ToSingle(Math.Sqrt((float)Math.Abs(((point1.X - point2.X) * (point1.X - point2.X)
+ (point1.Y - point2.Y) * (point1.Y - point2.Y))))));
}
/*Centroid of triangle is just an average of pointCloud
*/
public cPointi Centroid3(cPointi p1, cPointi p2, cPointi p3)
{
cPointi c = new cPointi();
c.X = p1.X + p2.X + p3.X;
c.Y = p1.Y + p2.Y + p3.Y;
return(c);
}
public cVertex(float i, float j)
{
Point = new cPointi();
Point.X = i;
Point.Y = j;
Point.Z = i * i + j * j;
PrevVertex = NextVertex = null;
}
public cVertex(float x, float y, float z)
{
Point = new cPointi();
Point.X = x;
Point.Y = y;
Point.Z = z;
PrevVertex = NextVertex = null;
}
//****************************************************
public float Length2(cPointi v)
/* Returns the squared distance in between two points*/
{
float ss;
ss = 0;
ss = (float)(v.X * v.X + v.Y * v.Y);
return(ss);
}
public cVertex()
{
PrevVertex = NextVertex = null;
Point = new cPointi();
IndexInPointCloud = 0;
Edge = null;
IsOnHull = false;
IsProcessed = false;
}
/*---------------------------------------------------------------------
* Returns the dot product of the two input vectors.
* ---------------------------------------------------------------------*/
private float Dot(cPointi a, cPointi b)
{
//int i;
float sum = 0.0f;
sum = a.X * b.X + a.Y * b.Y;
return(sum);
}
/*---------------------------------------------------------------------
* SegSegInt: Finds the point of intersection p between two closed
* segments ab and cd. Returns p and a char with the following meaning:
* 'e': The segments collinearly overlap, sharing a point.
* 'v': An endpoint (vertex) of one segment is on the other segment,
* but 'e' doesn't hold.
* '1': The segments intersect properly (i.e., they share a point and
* neither 'v' nor 'e' holds).
* '0': The segments do not intersect (i.e., they share no points).
* Note that two collinear segments that share just one point, an endpoint
* of each, returns 'e' rather than 'v' as one might expect.
* ---------------------------------------------------------------------*/
public char SegSegInt(cPointi a, cPointi b, cPointi c, cPointi d, cPointd p, cPointd q)
{
float s, t; /* The two parameters of the parametric eqns. */
float num, denom; /* Numerator and denoninator of equations. */
char code = '?'; /* Return char characterizing intersection. */
p.x = p.y = 100.0f; /* For testing purposes only... */
denom = a.X * (float)(d.Y - c.Y) +
b.X * (float)(c.Y - d.Y) +
d.X * (float)(b.Y - a.Y) +
c.X * (float)(a.Y - b.Y);
/* If denom is zero, then segments are parallel: handle separately. */
if (denom == 0)
{
return(ParallelInt(a, b, c, d, p, q));
}
num = a.X * (float)(d.Y - c.Y) +
c.X * (float)(a.Y - d.Y) +
d.X * (float)(c.Y - a.Y);
if ((num == 0) || (num == denom))
{
code = 'v';
}
s = num / denom;
System.Diagnostics.Debug.WriteLine("SegSegInt: num=" + num + ",denom=" + denom + ",s=" + s);
num = -(a.X * (float)(c.Y - b.Y) +
b.X * (float)(a.Y - c.Y) +
c.X * (float)(b.Y - a.Y));
if ((num == 0) || (num == denom))
{
code = 'v';
}
t = num / denom;
System.Diagnostics.Debug.WriteLine("SegSegInt: num=" + num + ",denom=" + denom + ",t=" + t);
if ((0.0 < s) && (s < 1) &&
(0.0 < t) && (t < 1))
{
code = '1';
}
else if ((0.0 > s) || (s > 1) ||
(0.0 > t) || (t > 1))
{
code = '0';
}
p.x = a.X + s * (b.X - a.X);
p.y = a.Y + s * (b.Y - a.Y);
return(code);
}
public void SetAChain(cPointd Jk, cPointi target)
{
cPointi vaux1;
cPointi vaux2;
vaux1 = new cPointi(list.head.Point.X, list.head.Point.Y);
vaux2 = new cPointi(target.X, target.Y);
list.ClearVertexList();
list.SetVertex(vaux1.X, vaux1.Y);
list.SetVertex((int)(Jk.x + .5), (int)(Jk.y + .5));
list.SetVertex(vaux2.X, vaux2.Y);
}
/* Draws a line and the quiry point of the InPoly request
*/
public void DrawInPoly(System.Drawing.Graphics g, cPointi queryP,
int CanW, int CanH, int w, int h)
{
//int k;
//g.setColor(System.Drawing.Color.Black);
//g.drawLine(0, queryP.y, CanW, queryP.y);
//for(k = 0; k < intCount; k++)
//{
// g.fillOval(GetInters(k)-(int)(w/2), queryP.y - (int)(h/2), w, h);
//}
//g.setColor(System.Drawing.Color.Red);
//g.fillOval(queryP.x - (int)(w/2), queryP.y - (int)(h/2), w, h);
}
private void Convolve()
{
int i = 0; /* Index into sorted edge vectors P */
int j = 0; /* Primary polygon indices */
cVertex v = P.head;
System.Diagnostics.Debug.WriteLine("Convolve: Start array i = " + i + ", primary j0=" + j0);
PutInOutput(p0.X, p0.Y);
i = 0; /* Start at angle -pi, rightward vector. */
j = j0; /* Start searching for j0. */
v = P.GetElement(i);
System.Diagnostics.Debug.WriteLine("Convolve, getElement(0)..." + v.Point.X + ", " + v.Point.Y);
do
{
/* Advance around secondary edges until next j reached. */
while (!(v.IsProcessed && v.IndexInPointCloud == j))
{
if (!v.IsProcessed)
{
p0 = AddVec(p0, v.Point);
PutInOutput(p0.X, p0.Y);
}
v = v.NextVertex;
i = (i + 1) % m;
// System.Diagnostics.Debug.WriteLine("X: i incremented to "+i);
}
/* Advance one primary edge. */
System.Diagnostics.Debug.WriteLine("X: j=" + j + " found at i=" + i);
p0 = AddVec(p0, v.Point);
PutInOutput(p0.X, p0.Y);
j = (j + 1) % n;
System.Diagnostics.Debug.WriteLine("X: j incremented to " + j);
} while (j != j0);
/* Finally, complete circuit on secondary/robot polygon. */
while (i != 0)
{
if (!v.IsProcessed)
{
p0 = AddVec(p0, v.Point);
PutInOutput(p0.X, p0.Y);
}
i = (i + 1) % m;
}
System.Diagnostics.Debug.WriteLine("X: i incremented to " + i + " in circuit");
}
/*---------------------------------------------------------------------
* Advances and prints out an inside vertex if appropriate.
* ---------------------------------------------------------------------*/
private cVertex Advance(cVertex a, String counter, bool inside, cPointi v)
{
if (inside)
{
InsertInters(v.X, v.Y);
}
if (counter.Equals("aa"))
{
aa++;
}
else if (counter.Equals("ba"))
{
ba++;
}
return(a.NextVertex);
}
public bool IntersectProp(cPointi a, cPointi b, cPointi c, cPointi d)
{
/* Eliminate improper cases. */
if (
Collinear(a, b, c) ||
Collinear(a, b, d) ||
Collinear(c, d, a) ||
Collinear(c, d, b))
{
return(false);
}
return
(Xor(Left(a, b, c), Left(a, b, d)) &&
Xor(Left(c, d, a), Left(c, d, b)));
}
/*---------------------------------------------------------------------
* Returns TRUE iff point c lies on the closed segement ab.
* Assumes it is already known that abc are collinear.
* (This is the only difference with Between().)
* ---------------------------------------------------------------------*/
public bool Between1(cPointi a, cPointi b, cPointi c)
{
//cPointi ba, ca;
/* If ab not vertical, check betweenness on x; else on y. */
if (a.X != b.X)
{
return(((a.X <= c.X) && (c.X <= b.X)) ||
((a.X >= c.X) && (c.X >= b.X)));
}
else
{
return(((a.Y <= c.Y) && (c.Y <= b.Y)) ||
((a.Y >= c.Y) && (c.Y >= b.Y)));
}
}
}// end Solve2
//---------------------------------------------------------------------
//TwoCircles finds an intersection point between two circles.
//General routine: no assumptions. Returns # of intersections; point in p.
//---------------------------------------------------------------------
public int TwoCircles(cPointi c1, float r1, cPointi c2, float r2, cPointd p)
{
cPointi c;
cPointd q;
int nsoln = -1;
// Translate so that c1={0,0}.
c = new cPointi(0, 0);
SubVec(c2, c1, c);
q = new cPointd(0, 0);
nsoln = TwoCircles0a(r1, c, r2, p);//p instead of
// Translate back.
p.x = p.x + c1.X;
p.y = p.y + c1.Y;
return(nsoln);
}
/* -------------------------------------------------------------------------
* The following set of routines compute the (real) intersection point between
* two segments. The two segments are taken to be the first four edges of
* the input "polygon" list. A character "code" is returned and printed out.
*/
public char SegSegTopLevel()
{
// Set the segments ab and cd to be the first four points in the list.
cVertex temp = list.head;
cPointi a = temp.Point;
temp = temp.NextVertex;
cPointi b = temp.Point;
temp = temp.NextVertex;
cPointi c = temp.Point;
temp = temp.NextVertex;
cPointi d = temp.Point;
// Store the results in class data field p, and returns code.
code = a.SegSegInt(a, b, c, d, this.p, q);
return(code);
}
}//end TwoCircles0a
//---------------------------------------------------------------------
//TwoCircles0b also assumes that the 1st circle is origin-centered.
//---------------------------------------------------------------------
public int TwoCircles0b(float r1, cPointi c2, float r2, cPointd p)
{
float a2; // center of 2nd circle when rotated to x-axis
cPointd q; // one solution when c2 on x-axis
float cost, sint; // sine and cosine of angle of c2
// Rotate c2 to a2 on x-axis.
a2 = Convert.ToSingle(Math.Sqrt(Length2(c2)));
cost = c2.X / a2;
sint = c2.Y / a2;
q = new cPointd(0, 0);
TwoCircles00(r1, a2, r2, q);
// Rotate back
p.x = cost * q.x + -sint * q.y;
p.y = sint * q.x + cost * q.y;
return(2);
}
//---------------------------------------------------------------------
//TwoCircles0a assumes that the first circle is centered on the origin.
//Returns # of intersections: 0, 1, 2, 3 (inf); point in p.
//-----------------------------------------------------------------------
public int TwoCircles0a(float r1, cPointi c2, float r2, cPointd p)
{
float dc2; // dist to center 2 squared
float rplus2, rminus2; // (r1 +/- r2)^2
float f; // fraction along c2 for nsoln=1
// Handle special cases.
dc2 = Length2(c2);
rplus2 = (r1 + r2) * (r1 + r2);
rminus2 = (r1 - r2) * (r1 - r2);
// No solution if c2 out of reach + or -.
if ((dc2 > rplus2) || (dc2 < rminus2))
{
return(0);
}
// One solution if c2 just reached.
// Then solution is r1-of-the-way (f) to c2.
if (dc2 == rplus2)
{
f = r1 / (float)(r1 + r2);
p.x = f * c2.X; p.y = f * c2.Y;
return(1);
}
if (dc2 == rminus2)
{
if (rminus2 == 0)
{ // Circles coincide.
p.x = r1; p.y = 0;
return(3);
}
f = r1 / (float)(r1 - r2);
p.x = f * c2.X; p.x = f * c2.Y;
return(1);
}
// Two intersections.
int auxint = TwoCircles0b(r1, c2, r2, p);
return(auxint);
}//end TwoCircles0a
/*---------------------------------------------------------------------
* Returns TRUE iff segments ab & cd intersect, properly or improperly.
*/
public bool Intersect(cPointi a, cPointi b, cPointi c, cPointi d)
{
if (IntersectProp(a, b, c, d))
{
return(true);
}
else if (Between(a, b, c) ||
Between(a, b, d) ||
Between(c, d, a) ||
Between(c, d, b))
{
return(true);
}
else
{
return(false);
}
}
public int TriangleSign(cPointi a, cPointi b, cPointi c)
{
float area2;
area2 = (b.X - a.X) * (float)(c.Y - a.Y) -
(c.X - a.X) * (float)(b.Y - a.Y);
/* The area should be an integer. */
if (area2 > 0.5)
{
return(1);
}
else if (area2 < -0.5)
{
return(-1);
}
else
{
return(0);
}
}
///*Returns the distance of the input point
// *from its perp. proj. to the e1 edge.
// *Uses method detailed in comp.graphics.algorithms FAQ
// */
public float DistEdgePoint(cPointi a, cPointi b, cPointi c)
{
float r, s;
float Length;
float dproj = 0.0f;
Length = Convert.ToSingle(Math.Sqrt(Math.Pow((b.X - a.X), 2) +
Math.Pow((b.Y - a.Y), 2)));
if (Length == 0)
{
System.Diagnostics.Debug.WriteLine("DistEdgePoint: Length = 0");
a.PrintPoint();
b.PrintPoint();
c.PrintPoint();
}
r = (((a.Y - c.Y) * (a.Y - b.Y))
- ((a.X - c.X) * (b.X - a.X))) / (Length * Length);
s = (((a.Y - c.Y) * (b.X - a.X))
- ((a.X - c.X) * (b.Y - a.Y))) / (Length * Length);
dproj = Math.Abs(s * Length);
// System.Diagnostics.Debug.WriteLine("XI = " + (a.x + r *(b.x-a.x))+" YI = "+(a.y+r*(b.y-a.y)));
if ((s != 0) && ((0.0 <= r) && (r <= 1)))
{
return(dproj);
}
if ((s == 0) && Between(a, b, c))
{
return(0.0f);
}
else
{
float ca = Dist(a, c);
float cb = Dist(b, c);
return(Math.Min(ca, cb));
}
}
private void Vectorize()
{
//int i;
cVertex v;
v = P.head;
System.Diagnostics.Debug.WriteLine("Vectorize: ");
System.Diagnostics.Debug.WriteLine("list before victorization");
P.PrintVertices();
cVertex startB = P.GetElement(n);
System.Diagnostics.Debug.WriteLine("startB !!!: ");
startB.PrintVertex();
SubVec(P.head.Point, startB.PrevVertex.Point, last);
do
{
cPointi c = SubVec(v.NextVertex.Point, v.Point);
System.Diagnostics.Debug.WriteLine("(" + v.NextVertex.Point.X + "," + v.NextVertex.Point.Y + ") - (" + v.Point.X + "," + v.Point.Y + ")");
v.Point.X = c.X;
v.Point.Y = c.Y;
v = v.NextVertex;
} while (v != startB.PrevVertex);
startB.PrevVertex.Point.X = last.X;
startB.PrevVertex.Point.Y = last.Y;
SubVec(startB.Point, P.head.PrevVertex.Point, last);
v = startB;
do
{
cPointi c = SubVec(v.NextVertex.Point, v.Point);
System.Diagnostics.Debug.WriteLine("(" + v.NextVertex.Point.X + "," + v.NextVertex.Point.Y + ") - (" + v.Point.X + "," + v.Point.Y + ")");
v.Point.X = c.X;
v.Point.Y = c.Y;
v = v.NextVertex;
} while (v != P.head.PrevVertex);
P.head.PrevVertex.Point.X = last.X;
P.head.PrevVertex.Point.Y = last.Y;
}
/* Finds polygon's center of gravity */
public void FindCG()
{
float A2, areaSum2 = 0; //partial area sum
cPointi Cent3 = new cPointi();
cVertex temp = list.head;
cPointi fixedv = list.head.Point;
this.CG.x = 0;
this.CG.y = 0;
do
{
Cent3 = fixedv.Centroid3(fixedv, temp.Point, temp.NextVertex.Point);
A2 = fixedv.Triangle2(fixedv, temp.Point, temp.NextVertex.Point);
this.CG.x = CG.x + (A2 * Cent3.X);
this.CG.y = CG.y + (A2 * Cent3.Y);
areaSum2 = areaSum2 + A2;
temp = temp.NextVertex;
} while (temp != list.head.PrevVertex);
//Division by 3 is delayed to the last moment.
this.CG.x = this.CG.x / (3 * areaSum2);
this.CG.y = this.CG.y / (3 * areaSum2);
}
public void MoveTo_i(cPointi p)
{
System.Diagnostics.Debug.WriteLine(" Move to i" + p.X + ", " + p.Y);
}
}//end Solve
public bool Solve3(float L1, float L2, float L3, cPointi target0)
{
cPointi target;
cPointd Jk; // coords of kinked joint returned by Solve2
cPointi J1; // Joint1 on x-axis
cPointi Ttarget; // translated target
//cPointi vaux1;
//cPointi vaux2;
target = new cPointi(target0.X, target0.Y);
System.Diagnostics.Debug.WriteLine("==>Solve3: links = " + L1 + ", " + L2 + ", " + L3);
Jk = new cPointd(0, 0);
if (Solve2(L1 + L2, L3, target, Jk))
{
firstlinks++;
nlinks = 2;
System.Diagnostics.Debug.WriteLine("Solve3: link1=" + (L1 + L2) + ", link2=" + L3 + ", joint=\n");
LineTo_d(Jk);
SetAChain(Jk, target);
return(true);
}
else if (Solve2(L1, L2 + L3, target, Jk))
{
System.Diagnostics.Debug.WriteLine("Solve3: link1= " + L1 + ", link2= " + (L2 + L3) + ", joint=\n");
nlinks = 2;
LineTo_d(Jk);
SetAChain(Jk, target);
return(true);
}
else
{ // pin J0 to 0.
// Shift so J1 is origin.
//J1.x = L1; J1.y = 0;
J1 = new cPointi(L1, 0);
Ttarget = new cPointi(0, 0);
SubVec(target, J1, Ttarget);
if (Solve2(L2, L3, Ttarget, Jk))
{
// Shift solution back to origin.
Jk.x += L1;
System.Diagnostics.Debug.WriteLine("Solve3: link1=" + L1 + ", link2= " + L2 + ", link3= " + L1 + ", joints=\n");
nlinks = 3;
LineTo_i(J1);
LineTo_d(Jk);
SetAChain(Jk, target);
cVertex VJ1 = new cVertex(list.head.Point.X + J1.X, list.head.Point.Y);
list.InsertBefore(VJ1, list.head.NextVertex);
return(true);
}
else
{
return(false);
}
}
}//end Solve3