public Edge(Edge e)
{
v = e.v;
this.p1 = e.p1;
this.p2 = e.p2;
this.pts = e.pts;
}
/// <summary>
/// rotate the face with the given angle, around the axis
/// </summary>
/// <param name="pt"></param>
/// <param name="alpha">in degree</param>
/// <param name="axis"></param>
/// <returns></returns>
public Pnt Rotate(Pnt pt, double alpha, string axis)
{
alpha = alpha * 2 * Math.PI / 360;
// results in here are a bit too much approximated
// it's even wrong as fk
if (axis == "z")
{
double a = pt.x * Math.Cos(alpha) - pt.y * Math.Sin(alpha);
double b = pt.x * Math.Sin(alpha) + pt.y * Math.Cos(alpha);
double c = pt.z;
return(new Pnt(a, b, c));
}
else if (axis == "y")
{
double a = pt.x * Math.Cos(alpha) + pt.z * Math.Sin(alpha);
double b = pt.y;
double c = -pt.x * Math.Sin(alpha) + pt.z * Math.Cos(alpha);;
return(new Pnt(a, b, c));
}
else if (axis == "x")
{
double a = pt.x;
double b = pt.y * Math.Cos(alpha) - pt.z * Math.Sin(alpha);
double c = pt.y * Math.Sin(alpha) + pt.z * Math.Cos(alpha);
return(new Pnt(a, b, c));
}
//Debug.Log("The Vector3 returned by Rotate() is wrong, u gave a wrong axis name: please use \"x\", \"y\" ou \"z\"");
return(new Pnt(0, 0, 0));
}
/// <summary>
/// returns a pnt rotated from angle degrees around the axis
/// </summary>
/// <param name="angle">in defree</param>
/// <param name="axis"></param>
/// <returns></returns>
public Pnt Rotate(double angle, Pnt axis)
{
Pnt p = this;
p.Rotated(angle, axis);
return(p);
}
// comparison
public bool IsEqual(Pnt p2)
{
if (this.x == p2.x && this.y == p2.y && this.z == p2.z)
{
return(true);
}
return(false);
}
// constructors
public Edge(List <gp_Pnt> pts)
{
this.pts = ToPnt(pts);
p1 = ToPnt(pts[0]);
p2 = ToPnt(pts[1]);
v = ToPnt(pts[1]) - ToPnt(pts[0]);
}
/// <summary>
/// doesn't verify that the point is included but verifies if the point is on the same plan
/// </summary>
/// <param name="p"></param>
/// <returns></returns>
public bool Contains(Pnt p)
{
if (pts.Count >= 3)
{
Edge e = new Edge(pts[0], p);
return(e.v.DotProduct(this.Normale) == 0);
}
return(true);
}
public Pnt(Pnt p)
{
x = p.x;
y = p.y;
z = p.z;
d.Clear();
d.Add(p.x);
d.Add(p.y);
d.Add(p.z);
}
public Edge(Pnt p1, Pnt p2)
{
v = p2 - p1;
this.pts.Clear();
this.pts.Add(p1);
this.pts.Add(p2);
this.p1 = p1;
this.p2 = p2;
}
public Edge(gp_Pnt p1, gp_Pnt p2)
{
v = ToPnt(p2) - ToPnt(p1);
this.pts.Clear();
this.pts.Add(ToPnt(p1));
this.pts.Add(ToPnt(p2));
this.p1 = ToPnt(p1);
this.p2 = ToPnt(p2);
}
/// <summary>
/// signed angle between two vectors defined by p1 and p2
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="axis">normale of the plane defined by the two vectors</param>
/// <returns></returns>
public static double SignedAngle(Pnt p1, Pnt p2, Pnt axis)
{
double angle = Math.Acos(DotProduct(Normalize(p1), Normalize(p2)));
Pnt cross = Cross(p1, p2);
if (DotProduct(axis, cross) < 0)
{ // Or > 0
angle = -angle;
}
return(angle * 360 / (2 * Math.PI)); // in degree
}
/// <summary>
/// checks if pts includes the Pnt pt, returns true in that case, false otherwise
/// </summary>
/// <param name="pt"></param>
/// <returns></returns>
public bool Includes(Pnt pt)
{
foreach (Pnt p in this.pts)
{
if (pt.Equals(p))
{
return(true);
}
}
return(false);
}
/// <summary>
/// modifies the edge
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
public void Replace(gp_Pnt p1, gp_Pnt p2)
{
v = ToPnt(p2) - ToPnt(p1);
this.pts.Clear();
this.pts.Add(ToPnt(p1));
this.pts.Add(ToPnt(p2));
this.p1 = ToPnt(p1);
this.p2 = ToPnt(p2);
}
/// <summary>
/// get the index of the vertex
/// </summary>
/// <param name="pt"></param>
/// <returns></returns>
public int Index(Pnt pt)
{
for (int i = 0; i < this.pts.Count; i++)
{
if (pts[i].Equals(pt))
{
return(i);
}
}
return(-1);
}
/// <summary>
/// translate the face with the vector p
/// </summary>
/// <param name="p"></param>
public void Translate(Pnt p)
{
for (int i = 0; i < pts.Count; i++)
{
pts[i] = new Pnt(pts[i] + p);
}
foreach (Face f in subFaces)
{
f.Translate(p);
}
}
/// <summary>
/// modifies the edge
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
public void Replace(Pnt p1, Pnt p2)
{
v = p2 - p1;
this.pts.Clear();
this.pts.Add(p1);
this.pts.Add(p2);
this.p1 = p1;
this.p2 = p2;
}
/// <summary>
/// retrieves the index of the vertex
/// </summary>
/// <param name="pt"></param>
/// <returns></returns>
public int RetrievesIndex(Pnt pt)
{
// make sure u applied Graham before
if (Index(pt) == -1)
{
this.Add(pt);
return(pts.Count - 1);
}
else
{
return(Index(pt));
}
}
/// <summary>
/// rotate this from the angle "angle" around the axis "axis"
/// </summary>
/// <param name="angle">in degree</param>
/// <param name="axis"></param>
public void Rotated(double angle, Pnt axis)
{
angle = angle * Math.PI / 180; // converting to radians
double rx = x * (Math.Cos(angle) + axis.x * axis.x * (1 - Math.Cos(angle))) + y * (axis.x * axis.y * (1 - Math.Cos(angle)) - axis.z * Math.Sin(angle)) + z * (axis.x * axis.z * (1 - Math.Cos(angle)) + axis.y * Math.Sin(angle));
double ry = x * (axis.y * axis.x * (1 - Math.Cos(angle)) + axis.z * Math.Sin(angle)) + y * (Math.Cos(angle) + axis.y * axis.y * (1 - Math.Cos(angle))) + z * (axis.y * axis.z * (1 - Math.Cos(angle)) - axis.x * Math.Sin(angle));
double rz = x * (axis.z * axis.x * (1 - Math.Cos(angle) - axis.y * Math.Sin(angle))) + y * (axis.z * axis.y * (1 - Math.Cos(angle)) + axis.x * Math.Sin(angle)) + z * (Math.Cos(angle) + axis.z * axis.z * (1 - Math.Cos(angle)));
x = rx;
y = ry;
z = rz;
d.Clear();
d.Add(rx);
d.Add(ry);
d.Add(rz);
}
/// <summary>
/// transforms to the new coordinate system with the 3 angles given
/// </summary>
/// <param name="face"></param>
/// <param name="alpha">in degree</param>
/// <param name="beta">in degree</param>
/// <param name="gamma">in degree</param>
/// <returns></returns>
private Face TransformToNewCoordinateSystem(Face face, double alpha, double beta, double gamma)
{
Face f = new Face();
foreach (Pnt pt in face.pts)
{
// first rotation
Pnt temp = Rotate(pt, alpha, "x");
// second rotation
temp = Rotate(temp, beta, "y");
// third rotation
temp = Rotate(temp, gamma, "z");
// add it to the new face
f.Add(temp);
}
return(f);
}
/// <summary>
/// dot product between this and the vector e2
/// </summary>
/// <param name="e2"></param>
/// <returns></returns>
public double DotProduct(Pnt e2)
{
return(x * e2.x + y * e2.y + z * e2.z);
}
/// <summary>
/// cross product between the two vectors
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <returns></returns>
public static Pnt Cross(Pnt p1, Pnt p2)
{
return(new Pnt(p1.y * p2.z - p1.z * p2.y, p1.z * p2.x - p1.x * p2.z, p1.x * p2.y - p1.y * p2.x));
}
/// <summary>
/// appplies graham on points
/// </summary>
/// <returns></returns>
public Face Graham()
{
// nothing verifies that points are really in the same face, be careful
// calculation of the new orthogonal coordinate system
Face f = new Face(this);
List <Pnt> c = f.CoordinateSystem; // coordinate system of our face
// calculation of the transformations into this coordinate system
double alpha = Pnt.Angle(new Pnt(c[0].x, c[0].y, 0), new Pnt(1, 0, 0)); // rotation around z axis on the plan X,Y between x' and x
// new Vector3(c[0].x, c[0].y,0) is the x vector with its z coordinates put to 0
f = TransformToNewCoordinateSystem(f, 0, 0, -alpha);
c = f.CoordinateSystem;
double beta = Pnt.Angle(new Pnt(0, c[2].y, c[2].z), new Pnt(0, 0, 1)); // rotation around the new x axis on the plan Y',Z'=Z between z'' and z'=z
// new Vector3(0, c[2].y, c[2].z) is the z vector with its x coordinate put to 0
f = TransformToNewCoordinateSystem(f, -beta, 0, 0);
c = f.CoordinateSystem;
double gamma = Pnt.Angle(new Pnt(c[0].x, 0, c[0].z), new Pnt(1, 0, 0)); // rotation around new y axis on the plan x,z between x'' and x'
f = TransformToNewCoordinateSystem(f, 0, -gamma, 0);
// http://ads.harvard.edu/books/1989fcm..book/Chapter2.pdf page 27 definitions of Goldstein
//new Face(new List<Pnt>() { f.pts[0], f.pts[1], f.pts[2] }).Normale.Log();
//new Face(new List<Pnt>() { f.pts[3], f.pts[1], f.pts[2] }).Normale.Log();
// we get the same results, which implies that they are on the same plan, which is kind of rassurant
// calculation of the superior hull (at this point it's like if points were in 2D )
List <int> ordonnedIndices = new List <int> {
0
}; // we decide that the first point will be also the first indice
bool gotMyFirstEdge = false;
//while (ordonnedIndices.Count < pts.Count) // as long as our indices are not all added... NONONO it must stop when we get back to our first point
while (ordonnedIndices[0] != ordonnedIndices[ordonnedIndices.Count - 1] || ordonnedIndices.Count == 1)
{
// we must get the first segment ( i mean the second point )
if (!gotMyFirstEdge)
{
int i = 1;
Edge e1 = new Edge(f.pts[0], f.pts[i]);
while (e1.Length <= 0)
{
if (i == f.pts.Count - 1) // we could not find any valid edge (Length>0)
{
return(null);
}
i++;
e1.Replace(f.pts[0], f.pts[i]);
}
int indice = i;
//we got our edge that has a valid length
// we got the first edge, but is this one good ? i mean: is it a border or is it crossing our face ?
Edge e2 = new Edge();
for (int j = i + 1; j < f.pts.Count; j++)
{
e2.Replace(f.pts[0], f.pts[j]);
if (e2.Length > 0)
{
double d = e1.Angle(e2, f.Normale);
// we got a valid edge, let's see if the angle is a good one to keep
if (e1.Angle(e2, f.Normale) > 0)
{ // if it turns right (or maybe left, but let's imagine it is right)
e1 = new Edge(e2); // we update e1, we found a better edge
indice = j;
}
}
}
// we got our edge, did we ?
if (e1.Length > 0)
{
// yes we did !! it's the most on the right (in regards of pts[0])
gotMyFirstEdge = true;
ordonnedIndices.Add(indice);
}
}
else // now that we got our first edge we must add all others
{
// we update our latest edge
Edge e1 = new Edge(f.pts[ordonnedIndices[ordonnedIndices.Count - 1]], f.pts[ordonnedIndices[ordonnedIndices.Count - 2]]);
double angle = 0;
int indice = 0;
// we find the new best match
for (int j = 0; j < f.pts.Count; j++)
{
Edge e2 = new Edge(f.pts[ordonnedIndices[ordonnedIndices.Count - 1]], f.pts[j]);
//Edge e2= new Edge(f.pts[1], f.pts[j]);
if (e2.Length > 0)
{
// we got a valid edge, let's see if the angle is a good one to keep
if (e1.Angle(e2, f.Normale) > angle && !e1.SameDirectionAndPoints(e2))
{ // if it turns right (or maybe left, but let's imagine it is right)
indice = j; // we update indice, we found a better edge
angle = e1.Angle(e2, f.Normale);
} // TODO maybe we need to modify the condition on the angle depending on the orientation of the face ?
}
}
// i hope u checked it was really a face otherwise u may get a weird result
ordonnedIndices.Add(indice);
}
}
List <Pnt> ordonnedPoints = new List <Pnt>();
foreach (int indice in ordonnedIndices)
{
ordonnedPoints.Add(pts[indice]); // we add all our points in the right order
}
Face g = new Face(ordonnedPoints.GetRange(0, ordonnedPoints.Count - 1)); // last point is the first point, we dont need it
if (!this.SameOrder(ordonnedPoints))
{// not same order
if (this.orientation == TopAbs_Orientation.TopAbs_FORWARD)
{
g.orientation = TopAbs_Orientation.TopAbs_REVERSED;
}
else
{
g.orientation = TopAbs_Orientation.TopAbs_FORWARD;
}
}
return(g);
}
public Face(Pnt v1, Pnt v2)
{
pts.Add(v1);
pts.Add(v2);
}
/// <summary>
/// dot product of the two vectors
/// </summary>
/// <param name="e1"></param>
/// <param name="e2"></param>
/// <returns></returns>
public static double DotProduct(Pnt e1, Pnt e2)
{
return(e1.x * e2.x + e1.y * e2.y + e1.z * e2.z);
}
/// <summary>
/// performs graham on the points of your face and returns triangles
/// </summary>
/// <param name="computeCompletely">true-> returns triangles / false -> only applies graham</param>
/// <returns>returns triangles</returns>
public Face ToTrianglesGraham(bool computeCompletely)
{
UniqueVertices();
Face f = Graham();
if (computeCompletely)
{
if (f.pts.Count == 4)
{
// __________________________________________________________
List <int> tempList1 = new List <int>();
Face tempFace1 = new Face();
tempList1.Add(f.RetrievesIndex(f.pts[0]));
tempFace1.Add(f.pts[0]);
tempList1.Add(f.RetrievesIndex(f.pts[1]));
tempFace1.Add(f.pts[1]);
tempList1.Add(f.RetrievesIndex(f.pts[3]));
tempFace1.Add(f.pts[3]);
tempFace1.orientation = f.orientation;
f.subFaces.Add(tempFace1);
f.triangles.AddRange(tempList1);
// __________________________________________________________
List <int> tempList2 = new List <int>();
Face tempFace2 = new Face();
tempList2.Add(f.RetrievesIndex(f.pts[1]));
tempFace2.Add(f.pts[1]);
tempList2.Add(f.RetrievesIndex(f.pts[2]));
tempFace2.Add(f.pts[2]);
tempList2.Add(f.RetrievesIndex(f.pts[3]));
tempFace2.Add(f.pts[3]);
tempFace2.orientation = f.orientation;
f.subFaces.Add(tempFace2);
f.triangles.AddRange(tempList2);
// __________________________________________________________
}
else
{
Pnt centroid = f.Centroid;
for (int i = 0; i < f.pts.Count - 1; i++) // the -1 may sound weird but it is because we are adding the centroid to the points ;)
{
List <int> tempList = new List <int>();
Face tempFace = new Face();
//
tempList.Add(f.RetrievesIndex(f.pts[i]));
tempFace.Add(f.pts[i]);
//
if (i == f.pts.Count - 1 - 1) // same reason here
{
tempList.Add(f.RetrievesIndex(f.pts[0]));
tempFace.Add(f.pts[0]);
}
else
{
tempList.Add(f.RetrievesIndex(f.pts[i + 1]));
tempFace.Add(f.pts[i + 1]);
}
//
tempList.Add(f.RetrievesIndex(centroid));
tempFace.Add(centroid);
//
tempFace.orientation = f.orientation;
f.subFaces.Add(tempFace);
f.triangles.AddRange(tempList);
}
} // looks like it never occurs
return(f); // ordonned points with triangles set
}
//*/
return(f); // only ordonned pts
}
/// <summary>
/// angle between two edges
/// </summary>
/// <param name="e"></param>
/// <param name="axis"></param>
/// <returns></returns>
public double Angle(Edge e, Pnt axis)
{
return(this.v.IsEqual(e.v) ? 0 : Pnt.SignedAngle(v, e.v, axis));
}
/// <summary>
/// returns the distance between this and pt
/// </summary>
/// <param name="pt"></param>
/// <returns></returns>
public double Distance(Pnt pt)
{
return(Math.Sqrt((x - pt.x) * (x - pt.x) + (y - pt.y) * (y - pt.y) + (z - pt.z) * (z - pt.z)));
}
/// <summary>
/// angle between the two vectors
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <returns></returns>
public static double Angle(Pnt p1, Pnt p2)
{
double angle = Math.Acos(DotProduct(Normalize(p1), Normalize(p2)));
return(angle * 360 / (2 * Math.PI)); // in degree;
}
/// <summary>
/// normalization of the vector
/// </summary>
/// <param name="p"></param>
/// <returns></returns>
public static Pnt Normalize(Pnt p)
{
double L = Math.Sqrt(p.x * p.x + p.y * p.y + p.z * p.z);
return(L == 0 ? p : p / L);
}
/// <summary>
/// add a point to the face
/// </summary>
/// <param name="v"></param>
public void Add(Pnt v)
{
pts.Add(v);
}
