private void CreateConcentric(AABRHalfEdge2 l, int slices, ref VertexRing lastSegment)
{
VertexRing segment = new VertexRing();
Vector3D v0 = new Vector3D(l.Origin.X, l.Origin.Y, 0);
Vector3D v1 = new Vector3D(l.Next.Origin.X, l.Next.Origin.Y, 0);
if (lastSegment != null)
{
lastSegment.next = segment;
segment.prev = lastSegment;
}
else
{
start = segment;
}
lastSegment = segment;
double angle = 2 * Math.PI / slices;
Vector3D v0Prev = v0;
if (EpsilonTests.IsNearlyZeroEpsHigh(v0.Y))
{
int index = this.AddVertex(v0.X, v0.Y, v0.Z);
segment.Ring.Add(index);
return;
}
for (int i = 0; i < slices; i++)
{
Vector3D vNext = new Vector3D(l.Origin.X, l.Origin.Y * Math.Sin(angle * i), l.Origin.Y * Math.Cos(angle * i));
int index = this.AddVertex(vNext.X, vNext.Y, vNext.Z);
segment.Ring.Add(index);
}
}
public void CreatePolygonWithNormals(AABRHalfEdge2 l, List<Vector3D> verts, int count, Bucket bucket)
{
if (verts.Count < 3)
throw new Exception("Too few vertices specified");
Vector3D v0 = verts[0];
Vector3D v1 = verts[1];
Vector3D v2 = verts[2];
List<Vertex3F> list;
bucket.ConcentricSegments.TryGetValue(l, out list);
if (list == null)
{
list = new List<Vertex3F>();
bucket.ConcentricSegments.Add(l, list);
}
// List<Vertex3F> lf = new List<Vertex3F>(verts.Count);
if (count == 3)
{
Vector3D normal = Vector3D.PlaneNormal(v0, v1, v2).Unit(); // important for lighting
verts.ForEach(x => list.Add(new Vertex3F((float)x.X, (float)x.Y, (float)x.Z, (float)normal.X, (float)normal.Y, (float)normal.Z)));
}
else if (count == 4)
{
Vector3D v3 = verts[3];
Vector3D n0 = new Vector3D(0, v0.Y, v0.Z).Unit();
Vector3D n1 = new Vector3D(0, v1.Y, v1.Z).Unit();
if (EpsilonTests.IsNearlyZeroEpsHigh(v0.X - v2.X))
{
Debug.Assert(EpsilonTests.IsNearlyZeroEpsHigh(v1.X - v3.X));
n0 = Vector3D.PlaneNormal(v0, v1, v2).Unit();
n1 = n0;
}
list.Add(new Vertex3F((float)v0.X, (float)v0.Y, (float)v0.Z, (float)n0.X, (float)n0.Y, (float)n0.Z));
list.Add(new Vertex3F((float)v1.X, (float)v1.Y, (float)v1.Z, (float)n1.X, (float)n1.Y, (float)n1.Z));
list.Add(new Vertex3F((float)v2.X, (float)v2.Y, (float)v2.Z, (float)n1.X, (float)n1.Y, (float)n1.Z));
list.Add(new Vertex3F((float)v0.X, (float)v0.Y, (float)v0.Z, (float)n0.X, (float)n0.Y, (float)n0.Z));
list.Add(new Vertex3F((float)v2.X, (float)v2.Y, (float)v2.Z, (float)n1.X, (float)n1.Y, (float)n1.Z));
list.Add(new Vertex3F((float)v3.X, (float)v3.Y, (float)v3.Z, (float)n0.X, (float)n0.Y, (float)n0.Z));
}
else
throw new Exception("The count was " + Environment.NewLine + count);
}
internal void NotifyPartZeroChanged(Vector3D trans)
{
object[] arr = new object[2];
m_Origins.partZero += trans;
arr[0] = trans;
arr[1] = CoordinateSystemType.PART_ZERO;
if (m_CoordinateSystemChangedEvent != null)
m_CoordinateSystemChangedEvent.Invoke(this, arr);
}
internal override void Translate(Vector3D v)
{
base.Translate(v);
CreateVertexArray();
}
internal override void Rotate(double angle, Vector3D direction)
{
base.Rotate(angle, direction);
CreateVertexArray();
}
public override void GlobalOffsetChanged(object sender, object offset)
{
m_GlobalOffset = offset as Vector3D;
}
/*
* author: Florian Ennemoser
* desc: Calculates the values for a circle segment.
* parameters: curCommand: The currend cnc command including x,y,i and k values
* prevCommand: The previous command where the tool is currently positioned
*/
public void ProcessCurve(Sentence curCommand, Sentence prevCommand, Queue<Vector3D> goBuffer, ref Vector3D absStart)
{
if (EpsilonTests.IsNearlyZeroEpsHigh(curCommand.I) && EpsilonTests.IsNearlyZeroEpsHigh(curCommand.K))
throw new Exception("Either I or K must be greater/less than zero");
double l = 0.00; // the distance between the start point on the circle segment and the end point on the circle segment
double r = Math.Sqrt(Math.Abs(curCommand.I) * Math.Abs(curCommand.I) + Math.Abs(curCommand.K) * Math.Abs(curCommand.K)); // circle radius
double arc = 0.00; //the arc
if (EpsilonTests.IsNearlyZeroEpsHigh(prevCommand.Y - curCommand.Y)) //special case: angle is 180°
{
if (EpsilonTests.IsNearlyZeroEpsHigh(curCommand.I)) //circle origin lies directly on the horizontal axis of start- end endpoint of circle
{
arc = 2 * r * Math.PI * 0.5;
}
else
{
l = Math.Abs(prevCommand.X - curCommand.X);
arc = Math.Acos(1 - Math.Pow(l, 2) / (2 * Math.Pow(r, 2))) * r;
}
}
else if (EpsilonTests.IsNearlyZeroEpsHigh(Math.Abs(prevCommand.X - curCommand.X) - Math.Abs(curCommand.K)) && EpsilonTests.IsNearlyZeroEpsHigh(Math.Abs(curCommand.I))) //special case: angle is 90° or 270°
{
if(IsGreaterHalfCircle())
arc = 2 * r * Math.PI * 0.75;
else
arc = 2 * r * Math.PI * 0.25;
}
else //angle is ]90°,180°[
{
l = Math.Sqrt(Math.Pow(Math.Abs(prevCommand.Y) - Math.Abs(curCommand.Y), 2) + Math.Pow(Math.Abs(prevCommand.X) - Math.Abs(curCommand.X), 2));
if (IsGreaterHalfCircle())
arc = 2 * r * Math.PI - Math.Acos(1 - Math.Pow(l, 2) / (2 * Math.Pow(r, 2))) * r;
else
arc = Math.Acos(1 - Math.Pow(l, 2) / (2 * Math.Pow(r, 2))) * r;
}
Debug.Assert(arc > 0, "The arc must be greater than 0");
Debug.Assert(r > 0, "The radius must be greater than 0");
// double segment = 12.566370614359172953850573533118;
// double segment = 6.28139;
double segment = 0.2;
int counter = 1;
double xPrev = Math.Abs(curCommand.K); //x-distance from circle center point to tool-position of last frame
double yPrev = Math.Abs(curCommand.I); //y-distance from circle center point to tool-position of last frame
Quadrant startQuadrant = GetQuadrant(curCommand.I, curCommand.K); //quadrant where arc starts
Quadrant curQuadrant = startQuadrant;
Quadrant prevQuadrant = startQuadrant;
double alpha = CalcStartAngle(curCommand.interpolationMode, r, startQuadrant, curCommand.I);
Vector3D amount;
while (segment * counter < arc || EpsilonTests.IsNearlyZeroEpsHigh(arc - segment * counter))
{
alpha += segment / r;
GetQuadrant(alpha, startQuadrant, ref curQuadrant, ref prevQuadrant, curCommand.interpolationMode);
double deltaX = calcX(ref xPrev, alpha, r, startQuadrant, curQuadrant, prevQuadrant, curCommand.interpolationMode);
double deltaY = calcY(ref yPrev, alpha, r, startQuadrant, curQuadrant, prevQuadrant, curCommand.interpolationMode);
prevQuadrant = curQuadrant;
amount = new Vector3D(deltaX,deltaY,0);
if (!EpsilonTests.IsGreaterEpsHigh(amount.Length()))
throw new Exception("Length too small: " + amount.Length());
goBuffer.Enqueue(amount);
counter++;
}
double arcLeft = arc - segment * (counter-1);
if (!EpsilonTests.IsNearlyZeroEpsHigh(arcLeft))
{
alpha += arcLeft / r;
GetQuadrant(alpha, startQuadrant, ref curQuadrant, ref prevQuadrant, curCommand.interpolationMode);
double deltaX = calcX(ref xPrev, alpha, r, startQuadrant, curQuadrant, prevQuadrant, curCommand.interpolationMode);
double deltaY = calcY(ref yPrev, alpha, r, startQuadrant, curQuadrant, prevQuadrant, curCommand.interpolationMode);
amount = new Vector3D(deltaX, deltaY, 0);
double l1 = amount.Length();
if (amount.Length() >= 0.0001)
goBuffer.Enqueue(amount);
else
{
Vector3D last = goBuffer.Last();
last.X += amount.X;
last.Y += amount.Y;
Debug.Assert(last.Z == 0);
}
}
#if DEBUG
double overallX = 0;
double overallY = 0;
foreach (Vector3D v in goBuffer)
{
overallX += v.X;
overallY += v.Y;
}
#endif
absStart = new Vector3D(curCommand.X,curCommand.Y,0);
}
internal virtual void Translate(Vector3D v)
{
if (v.X != 0.0 || v.Y != 0.0 || v.Z != 0.0)
{
foreach (HalfEdge3Vertex vert in m_Mesh.Vertices)
{
vert.X += v.X;
vert.Y += v.Y;
vert.Z += v.Z;
}
m_transformationMatrix.Translate(v);
}
}
private static void GetVector2Values(string value, Vector3D vec, LogWriter outputWriter)
{
value = value.Trim();
value = Regex.Replace(value, @"\s", "");
if (value[0] != '(' || value[value.Length - 1] != ')')
{
outputWriter.Write("Origin not properly specified in initializer.conf" + Environment.NewLine, "Output", -1, -1, System.Diagnostics.TraceEventType.Error);
return;
}
string[] numbers = Regex.Split(value, @"\D+");
List<int> nums = new List<int>();
foreach (string v in numbers)
{
if (!string.IsNullOrEmpty(v))
{
int i = int.Parse(v);
nums.Add(i);
}
}
if (nums.Count != 2)
throw new ArgumentException();
vec.X = nums[0]; vec.Y = nums[1];
}
public RegularRectangle(RegularRectangle source)
{
starting_point = new Point2D(source.starting_point.X, source.starting_point.Y);
diagonal = new Vector3D(source.diagonal.X, source.diagonal.Y, 0);
}
internal BVHPolyMesh2D FeedRate2D(Tool tool, Vector3D direction)
{
BVHPolyMesh2D newMesh = new BVHPolyMesh2D();
List<int> indices = tool.CuttingEdgePolyIndices;
HalfEdge3Poly p = tool.Mesh.Polys[indices[0]];
Debug.Assert(EpsilonTests.IsNearlyZeroEpsHigh(direction.Z));
DataStructures.Tuple<double, HalfEdge3> min = new DataStructures.Tuple<double, HalfEdge3>(Double.MaxValue, null);
DataStructures.Tuple<double, HalfEdge3> max = new DataStructures.Tuple<double, HalfEdge3>(Double.MinValue, null);
// build 2d TSV for this poly
//calc min and max vertex
foreach (int i in tool.Mesh.GetFaceCirculator(p.OuterComponent))
{
HalfEdge3 h = tool.Mesh.HalfEdges[i];
Debug.Assert(EpsilonTests.IsNearlyZeroEpsHigh(tool.Mesh.Vertices[h.Origin].Z));
double c = direction.Y * tool.Mesh.Vertices[h.Origin].X - direction.X * tool.Mesh.Vertices[h.Origin].Y;
if (c < min.Val1)
{
min.Val1 = c;
min.Val2 = h;
}
if (c >= max.Val1)
{
max.Val1 = c;
max.Val2 = h;
}
}
Debug.Assert(min.Val2 != null && max.Val2 != null);
Debug.Assert(min.Val2 != max.Val2);
// Create tsv of poly
List<HEVector2> vertexList = new List<HEVector2>();
bool onFrontFace = true;
HalfEdge3 start, it;
it = start = max.Val2;
do
{
if (it == max.Val2)
{
vertexList.Add(new HEVector2(tool.Mesh.Vertices[it.Origin].X, tool.Mesh.Vertices[it.Origin].Y));
}
if (onFrontFace)
{
vertexList.Add(new HEVector2(tool.Mesh.Vertices[it.Origin].X + direction.X, tool.Mesh.Vertices[it.Origin].Y + direction.Y));
}
else
{
vertexList.Add(new HEVector2(tool.Mesh.Vertices[it.Origin].X, tool.Mesh.Vertices[it.Origin].Y));
}
if (it == min.Val2)
{
vertexList.Add(new HEVector2(tool.Mesh.Vertices[it.Origin].X, tool.Mesh.Vertices[it.Origin].Y));
onFrontFace = false;
}
it = tool.Mesh.HalfEdges[it.Next];
}
while (it != start);
newMesh.CreateMeshFromVertices(vertexList);
newMesh.CreateBVH();
return newMesh;
}
public RegularRectangle(double top, double bottom, double right, double left)
{
starting_point = new Point2D(left, bottom);
diagonal = new Vector3D(right - left, top - bottom, 0);
}
public RegularRectangle(Point2D p1, Point2D p2)
{
starting_point = new Point2D(p1.X < p2.X ? p1.X : p2.X, p1.Y < p2.Y ? p1.Y : p2.Y);
diagonal = new Vector3D(Math.Abs(p1.X - p2.X), Math.Abs(p1.Y - p2.Y), 0);
}
/// <summary>
/// ベクトルのクロス積
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static Vector3D CrossProduct(Vector3D a, Vector3D b)
{
return new Vector3D(a.Y * b.Z - a.Z * b.Y, a.Z * b.X - a.X * b.Z, a.X * b.Y - a.Y * b.X);
}
internal void NotifyToolZeroChanged(Vector3D trans)
{
object[] arr = new object[2];
arr[0] = trans;
arr[1] = CoordinateSystemType.TOOL_ZERO;
if (m_CoordinateSystemChangedEvent != null)
m_CoordinateSystemChangedEvent.Invoke(this, arr);
}
private void CreateConcentric(AABRHalfEdge2 l, int slices, Bucket bucket)
{
//slices = 4;
Vector3D v0 = new Vector3D(l.Origin.X, 0, l.Origin.Y);
Vector3D v1 = new Vector3D(l.Next.Origin.X, 0, l.Next.Origin.Y);
if (EpsilonTests.IsNearlyZeroEpsHigh(l.Origin.Y) && EpsilonTests.IsNearlyZeroEpsHigh(l.Next.Origin.Y))
return;
double angle = 2 * Math.PI / slices;
Vector3D v0Prev = v0;
Vector3D v1Prev = v1;
Vector3D v0t0 = new Vector3D(l.Origin.X, 0, l.Origin.Y);
Vector3D v1t0 = new Vector3D(l.Next.Origin.X, 0, l.Next.Origin.Y);
Vector3D v0t1 = new Vector3D(l.Origin.X, l.Origin.Y * Math.Sin(angle), l.Origin.Y * Math.Cos(angle));
Vector3D v1t1 = new Vector3D(l.Next.Origin.X, l.Next.Origin.Y * Math.Sin(angle), l.Next.Origin.Y * Math.Cos(angle));
Vector3D v0t2 = new Vector3D(l.Origin.X, l.Origin.Y * Math.Sin(angle * 2), l.Origin.Y * Math.Cos(angle * 2));
Vector3D v1t2 = new Vector3D(l.Next.Origin.X, l.Next.Origin.Y * Math.Sin(angle * 2), l.Next.Origin.Y * Math.Cos(angle * 2));
Vector3D normal0 = Vector3D.PlaneNormal(v0t0, v0t1, v1t0).Unit();
Vector3D normal1 = Vector3D.PlaneNormal(v0t1, v0t2, v1t1).Unit();
double p0 = l.Origin.Y;
double p1 = l.Next.Origin.Y;
List<Vertex3F> list;
bucket.ConcentricSegments.TryGetValue(l, out list);
if (list == null)
{
list = new List<Vertex3F>();
bucket.ConcentricSegments.Add(l, list);
}
for (int j = 0, i = 3; j < slices; j++, i++, i %= slices)
{
Vector3D v0t3 = new Vector3D(l.Origin.X, l.Origin.Y * Math.Sin(angle * i), l.Origin.Y * Math.Cos(angle * i));
Vector3D v1t3 = new Vector3D(l.Next.Origin.X, l.Next.Origin.Y * Math.Sin(angle * i), l.Next.Origin.Y * Math.Cos(angle * i));
Vector3D normal2 = Vector3D.PlaneNormal(v0t2, v0t3, v1t2).Unit();
Vector3D n0 = (normal0 + normal1).Unit();
Vector3D n1 = (normal1 + normal2).Unit();
if (EpsilonTests.IsNearlyZeroEpsHigh(p0)) // create polys with 3 vertices
{
normal0 = Vector3D.PlaneNormal(v0t0, v1t1, v1t0);
normal1 = Vector3D.PlaneNormal(v0t0, v1t2, v1t1);
normal2 = Vector3D.PlaneNormal(v0t0, v1t3, v1t2);
n0 = (normal0 + normal1).Unit();
n1 = (normal1 + normal2).Unit();
}
else if (EpsilonTests.IsNearlyZeroEpsHigh(p1)) // create polys with 3 vertices
{
normal0 = Vector3D.PlaneNormal(v0t0, v0t1, v1t0);
normal1 = Vector3D.PlaneNormal(v0t1, v0t2, v1t0);
normal2 = Vector3D.PlaneNormal(v0t2, v0t3, v1t0);
n0 = (normal0 + normal1).Unit();
n1 = (normal1 + normal2).Unit();
}
else // create polys with 4 vertices
{
normal0 = Vector3D.PlaneNormal(v0t0, v0t1, v1t0);
normal1 = Vector3D.PlaneNormal(v0t1, v0t2, v1t1);
normal2 = Vector3D.PlaneNormal(v0t2, v0t3, v1t2);
n0 = (normal0 + normal1).Unit();
n1 = (normal1 + normal2).Unit();
}
list.Add(new Vertex3F((float)v0t1.X, (float)v0t1.Y, (float)v0t1.Z, (float)n0.X, (float)n0.Y, (float)n0.Z));
list.Add(new Vertex3F((float)v0t2.X, (float)v0t2.Y, (float)v0t2.Z, (float)n1.X, (float)n1.Y, (float)n1.Z));
list.Add(new Vertex3F((float)v1t1.X, (float)v1t1.Y, (float)v1t1.Z, (float)n0.X, (float)n0.Y, (float)n0.Z));
list.Add(new Vertex3F((float)v0t2.X, (float)v0t2.Y, (float)v0t2.Z, (float)n1.X, (float)n1.Y, (float)n1.Z));
list.Add(new Vertex3F((float)v1t2.X, (float)v1t2.Y, (float)v1t2.Z, (float)n1.X, (float)n1.Y, (float)n1.Z));
list.Add(new Vertex3F((float)v1t1.X, (float)v1t1.Y, (float)v1t1.Z, (float)n0.X, (float)n0.Y, (float)n0.Z));
v0t0 = v0t1;
v1t0 = v1t1;
v0t1 = v0t2;
v1t1 = v1t2;
v0t2 = v0t3;
v1t2 = v1t3;
normal0 = normal1;
normal1 = normal2;
}
}
internal OGLCoordinateAxis(Vector3D offset)
{
Offset = offset;
}
internal virtual void Rotate(double angle, Vector3D direction)
{
Vector3D d = direction.Unit();
double x = d.X;
double y = d.Y;
double z = d.Z;
TransformationMatrix m = new TransformationMatrix();
double c = Math.Cos(angle);
double s = Math.Sin(angle);
m[0,0] = (x*x) * (1-c)+c;
m[1,0] = x*y * (1-c)-z*s;
m[2,0] = x*z * (1-c)+y*s;
m[0,1] = y*x * (1-c)+z*s;
m[1,1] = (y*y) * (1-c)+c;
m[2,1] = y*z * (1-c)-x*s;
m[0,2] = x*z * (1-c)-y*s;
m[1,2] = y*z * (1-c)+x*s;
m[2,2] = (z*z) * (1-c)+c;
UpdateLocation(m);
m = m.Invert().Transpose();
foreach (Vector3D v in m_Mesh.Normals)
{
double x0 = m[0, 0] * v.X + m[0, 1] * v.Y + m[0, 2] * v.Z + m[0, 3];
double y0 = m[1, 0] * v.X + m[1, 1] * v.Y + m[1, 2] * v.Z + m[1, 3];
double z0 = m[2, 0] * v.X + m[2, 1] * v.Y + m[2, 2] * v.Z + m[2, 3];
v.X = x0; v.Y = y0; v.Z = z0;
}
}
