public void DrawByLines(OpenGL gl)
{
lock (locker)
{
int n = (int)Math.Sqrt(vertices.Length);
for (int i = 0; i < n; ++i)
{
gl.Begin(OpenGL.GL_LINE_STRIP);
for (int j = 0; j < n; ++j)
{
DVector4 p = vertices[i * n + j]._Point;
gl.Vertex(p.X, p.Y, p.Z);
}
gl.End();
}
for (int i = 0; i < n; ++i)
{
gl.Begin(OpenGL.GL_LINE_STRIP);
for (int j = 0; j < n; ++j)
{
DVector4 p = vertices[j * n + i]._Point;
gl.Vertex(p.X, p.Y, p.Z);
}
gl.End();
}
}
}
public int Accept(DVector4 data, TType type, Title x)
{
var v = data.Value;
SetTitleValue(x, $"{v.X},{v.Y},{v.Z},{v.W}");
return(1);
}
public Vertex(DVector3 point)
{
Polygon = new List <Polygon>();
_Point = new DVector4(point, 1.0);
_Normal = DVector4.Zero;
}
// деконструкция в отдельные переменные: var (x, y, z, w) = vector
public static void Deconstruct(this DVector4 vector, out double x, out double y, out double z, out double w)
{
x = vector.X;
y = vector.Y;
z = vector.Z;
w = vector.W;
}
private static DVector4 CrossProduct(DVector4 a, DVector4 b)
{
double X = a.Y * b.Z - a.Z * b.Y;
double Y = a.Z * b.X - a.X * b.Z;
double Z = a.X * b.Y - a.Y * b.X;
return(new DVector4(X, Y, Z, 0.0));
}
// преобразование в DVector3 с делением на W, если это точка (т.е. W != 0)
// или с простым отбрасыванием W, если это вектор (т.е. W == 0)
public static DVector3 ToDVector3(this DVector4 vector)
{
if (vector.W == 0)
{
return(new DVector3(vector.X, vector.Y, vector.Z));
}
return(new DVector3(vector.X, vector.Y, vector.Z) / vector.W);
}
// вершины по часовой стрелке
public Polygon(DVector4 p1, DVector4 p2, DVector4 p3)
{
P1 = p1;
P2 = p2;
P3 = p3;
Normal = DVector3.CrossProduct(P3 - P1, P2 - P1)
.Normalized();
}
public void Accept(DVector4 type, Utf8JsonWriter x)
{
x.WriteStartObject();
x.WriteNumber("x", type.Value.X);
x.WriteNumber("y", type.Value.Y);
x.WriteNumber("z", type.Value.Z);
x.WriteNumber("w", type.Value.W);
x.WriteEndObject();
}
protected void Prism()
{
Vertecis = new Vertex[10];
Polygons = new Polygon[7];
for (int i = 0; i < 5; ++i)
{
double RotStep = 2.0 * Math.PI / 5.0;
double x = Math.Cos(i * RotStep);
double y = Math.Sin(i * RotStep);
Vertecis[i] = new Vertex(new DVector3(x, y, -0.5));
Vertecis[i + 5] = new Vertex(new DVector3(x, y, 0.5));
}
for (int i = 1; i < 5; ++i)
{
Polygons[i] = new Polygon(new List <Vertex>()
{
Vertecis[i - 1], Vertecis[i], Vertecis[i + 5], Vertecis[i - 1 + 5]
});
}
Polygons[0] = new Polygon(new List <Vertex>()
{
Vertecis[4], Vertecis[0], Vertecis[5], Vertecis[9]
});
List <Vertex> lowerBase = new List <Vertex>(5);
for (int i = 0; i < 5; ++i)
{
lowerBase.Add(Vertecis[i]);
}
lowerBase.Reverse();
Polygons[5] = new Polygon(lowerBase);
List <Vertex> upperBase = new List <Vertex>(5);
for (int i = 0; i < 5; ++i)
{
upperBase.Add(Vertecis[i + 5]);
}
Console.WriteLine(upperBase.Count);
Polygons[6] = new Polygon(upperBase);
foreach (Vertex vert in Vertecis)
{
DVector4 norm = DVector4.Zero;
foreach (Polygon pol in vert.Polygon)
{
norm += pol._Normal;
}
vert._Normal = norm / vert.Polygon.Count;
}
}
protected void Tetrahedron()
{
Vertecis = new Vertex[4];
Polygons = new Polygon[4];
double sqrt3 = Math.Sqrt(3);
double sqrt6 = Math.Sqrt(6);
Vertecis[0] = new Vertex(new DVector3(0.0, 0.0, 0.0));
Vertecis[1] = new Vertex(new DVector3(0.5, sqrt3 / 2.0, 0.0));
Vertecis[2] = new Vertex(new DVector3(1.0, 0.0, 0.0));
Vertecis[3] = new Vertex(new DVector3(0.5, sqrt3 / 6.0, sqrt6 / 3.0));
DVector4 center = Vertecis[0]._Point;
for (int i = 1; i < 4; ++i)
{
center += Vertecis[i]._Point;
}
center /= 4.0;
foreach (Vertex v in Vertecis)
{
v._Point -= center;
}
Polygons[0] = new Polygon(new List <Vertex>()
{
Vertecis[0], Vertecis[1], Vertecis[2]
});
Polygons[1] = new Polygon(new List <Vertex>()
{
Vertecis[0], Vertecis[2], Vertecis[3]
});
Polygons[2] = new Polygon(new List <Vertex>()
{
Vertecis[1], Vertecis[0], Vertecis[3]
});
Polygons[3] = new Polygon(new List <Vertex>()
{
Vertecis[2], Vertecis[1], Vertecis[3]
});
foreach (Vertex vert in Vertecis)
{
DVector4 norm = DVector4.Zero;
foreach (Polygon pol in vert.Polygon)
{
norm += pol._Normal;
}
vert._Normal = norm / vert.Polygon.Count;
}
}
public Polygon(List <Vertex> verts)
{
Vertex = verts;
_Normal = CrossProduct(verts[0]._Point - verts[1]._Point, verts[1]._Point - verts[2]._Point);
_Normal /= _Normal.GetLength();
foreach (Vertex v in verts)
{
v.Polygon.Add(this);
}
}
void DrawVertexNumbers(Polygon p, Graphics g, double Width, double Height)
{
foreach (var v in p.Vertex)
{
for (int i = 0; i < Vertecis.Length; ++i)
{
if (DVector4.ApproxEqual(v.Point, Vertecis[i].Point))
{
g.DrawString(i.ToString(), new Font("Arial", 10f), Brushes.Black, (float)(v.Point.X + Width * 0.5 + ShiftX), (float)(v.Point.Y + Height * 0.5 - ShiftY));
break;
}
}
}
}
void DrawPolygonFixGaps2(Polygon p, Graphics g, double Width, double Height)
{
DVector3 center = PolygonCenter(p);
center.X += Width * 0.5;
center.Y += Height * 0.5;
int count = p.Vertex.Count;
PointF[] pp = new PointF[count];
for (int i = 0; i < count; ++i)
{
DVector4 v = p.Vertex[i].Point;
double x = v.X + Width * 0.5 + ShiftX;
double y = v.Y + Height * 0.5 - ShiftY;
float allowance = 0.0f;
if (x > center.X)
{
x = Math.Ceiling(x) + allowance;
}
if (y > center.Y)
{
y = Math.Ceiling(y) + allowance;
}
if (x < center.X)
{
x = Math.Floor(x) - allowance;
}
if (y < center.Y)
{
y = Math.Floor(y) - allowance;
}
pp[i] = new PointF((float)x, (float)y);
}
g.FillPolygon(Brushes.Red, pp);
if (EnableWireframe)
{
for (int i = 0; i < count - 1; ++i)
{
g.DrawLine(Pens.White, pp[i], pp[i + 1]);
}
g.DrawLine(Pens.White, pp[count - 1], pp[0]);
}
}
/// <summary>
/// Constructs an ApiPoseData equivalent to a unity PoseData.
/// </summary>
public ApiPoseData(UnityTango.PoseData unityPoseData)
{
// TODO (mtsmall): Tell unity version and accuracy should be uint.
version = (int)unityPoseData.version;
timestamp = unityPoseData.timestamp;
orientation = new DVector4(unityPoseData.orientation_x, unityPoseData.orientation_y,
unityPoseData.orientation_z, unityPoseData.orientation_w);
translation = new DVector3(unityPoseData.orientation_x, unityPoseData.orientation_y,
unityPoseData.orientation_z);
statusCode = unityPoseData.statusCode.ToApiType();
framePair = new ApiCoordinateFramePair(unityPoseData.frame);
confidence = (int)unityPoseData.confidence;
accuracy = unityPoseData.accuracy;
}
protected void Cube()
{
Vertecis = new Vertex[8];
Polygons = new Polygon[6];
Vertecis[0] = new Vertex(new DVector3(0.5, -0.5, -0.5));
Vertecis[1] = new Vertex(new DVector3(0.5, 0.5, -0.5));
Vertecis[2] = new Vertex(new DVector3(-0.5, 0.5, -0.5));
Vertecis[3] = new Vertex(new DVector3(-0.5, -0.5, -0.5));
Vertecis[4] = new Vertex(new DVector3(0.5, -0.5, 0.5));
Vertecis[5] = new Vertex(new DVector3(0.5, 0.5, 0.5));
Vertecis[6] = new Vertex(new DVector3(-0.5, 0.5, 0.5));
Vertecis[7] = new Vertex(new DVector3(-0.5, -0.5, 0.5));
Polygons[0] = new Polygon(new List <Vertex>()
{
Vertecis[0], Vertecis[1], Vertecis[5], Vertecis[4]
});
Polygons[1] = new Polygon(new List <Vertex>()
{
Vertecis[1], Vertecis[2], Vertecis[6], Vertecis[5]
});
Polygons[2] = new Polygon(new List <Vertex>()
{
Vertecis[2], Vertecis[3], Vertecis[7], Vertecis[6]
});
Polygons[3] = new Polygon(new List <Vertex>()
{
Vertecis[3], Vertecis[0], Vertecis[4], Vertecis[7]
});
Polygons[4] = new Polygon(new List <Vertex>()
{
Vertecis[3], Vertecis[2], Vertecis[1], Vertecis[0]
});
Polygons[5] = new Polygon(new List <Vertex>()
{
Vertecis[4], Vertecis[5], Vertecis[6], Vertecis[7]
});
foreach (Vertex vert in Vertecis)
{
DVector4 norm = DVector4.Zero;
foreach (Polygon pol in vert.Polygon)
{
norm += pol._Normal;
}
vert._Normal = norm / vert.Polygon.Count;
}
}
public void DrawByQuads(OpenGL gl)
{
lock (locker)
{
gl.Begin(OpenGL.GL_QUADS);
foreach (Polygon pol in polygons)
{
foreach (Vertex vert in pol.Vertex)
{
DVector4 point = vert._Point;
gl.Vertex(point.X, point.Y, point.Z);
}
}
gl.End();
}
}
private void DrawAxis(Graphics g, DVector4 ox, DVector4 oy, DVector4 oz, DVector4 pos)
{
Pen pr = new Pen(Color.Red, 2.0f);
Pen pg = new Pen(Color.Green, 2.0f);
Pen pb = new Pen(Color.Blue, 2.0f);
// Ox
g.DrawLine(pr, (float)(pos.X), (float)(pos.Y), (float)(pos.X + ox.X), (float)(pos.Y + ox.Y));
// Oy
g.DrawLine(pg, (float)(pos.X), (float)(pos.Y), (float)(pos.X + oy.X), (float)(pos.Y + oy.Y));
// Oz
g.DrawLine(pb, (float)(pos.X), (float)(pos.Y), (float)(pos.X + oz.X), (float)(pos.Y + oz.Y));
g.DrawString("x", new Font("Arial", 10f), Brushes.Red, (float)(pos.X + ox.X + 3), (float)(pos.Y + ox.Y + 3));
g.DrawString("y", new Font("Arial", 10f), Brushes.Green, (float)(pos.X + oy.X + 3), (float)(pos.Y + oy.Y + 3));
g.DrawString("z", new Font("Arial", 10f), Brushes.Blue, (float)(pos.X + oz.X + 3), (float)(pos.Y + oz.Y + 3));
}
protected void Tetrahedron()
{
Vertecis = new Vertex[4];
Polygons = new Polygon[4];
double sqrt3 = Math.Sqrt(3);
double sqrt6 = Math.Sqrt(6);
Vertecis[0] = new Vertex(new DVector3(0.0, 0.0, 0.0));
Vertecis[1] = new Vertex(new DVector3(0.5, sqrt3 / 2.0, 0.0));
Vertecis[2] = new Vertex(new DVector3(1.0, 0.0, 0.0));
Vertecis[3] = new Vertex(new DVector3(0.5, sqrt3 / 6.0, sqrt6 / 3.0));
DVector4 center = Vertecis[0]._Point;
for (int i = 1; i < 4; ++i)
{
center += Vertecis[i]._Point;
}
center /= 4.0;
foreach (Vertex v in Vertecis)
{
v._Point -= center;
}
Polygons[0] = new Polygon(new List <Vertex>()
{
Vertecis[0], Vertecis[1], Vertecis[2]
});
Polygons[1] = new Polygon(new List <Vertex>()
{
Vertecis[0], Vertecis[2], Vertecis[3]
});
Polygons[2] = new Polygon(new List <Vertex>()
{
Vertecis[1], Vertecis[0], Vertecis[3]
});
Polygons[3] = new Polygon(new List <Vertex>()
{
Vertecis[2], Vertecis[1], Vertecis[3]
});
}
/// <summary>
/// Given a screen coordinate, finds a plane that most closely fits the
/// depth values in that area.
///
/// This function is slow, as it looks at every single point in the point
/// cloud. Avoid calling this more than once a frame. This also assumes the
/// Unity camera intrinsics match the device's color camera.
/// </summary>
/// <returns><c>true</c>, if a plane was found; <c>false</c> otherwise.</returns>
/// <param name="cam">The Unity camera.</param>
/// <param name="pos">The point in screen space to perform detection on.</param>
/// <param name="planeCenter">Filled in with the center of the plane in Unity world space.</param>
/// <param name="plane">Filled in with a model of the plane in Unity world space.</param>
public bool FindPlane(Camera cam, Vector2 pos, out Vector3 planeCenter, out Plane plane)
{
if (m_pointsCount == 0)
{
// No points to check, maybe not connected to the service yet
planeCenter = Vector3.zero;
plane = new Plane();
return(false);
}
Vector2 normalizedPos = cam.ScreenToViewportPoint(pos);
// If the camera has a TangoARScreen attached, it is not displaying the entire color camera image. Correct
// the normalized coordinates by taking the clipping into account.
TangoARScreen arScreen = cam.gameObject.GetComponent <TangoARScreen>();
if (arScreen != null)
{
normalizedPos = arScreen.ViewportPointToCameraImagePoint(normalizedPos);
}
DVector4 planeModel = new DVector4();
bool returnValue = TangoSupport.FitPlaneModelNearClick(
m_mostRecentPointCloud,
arScreen.m_screenUpdateTime,
normalizedPos,
out planeCenter,
out planeModel);
planeCenter = m_mostRecentUnityWorldTDepthCamera.MultiplyPoint3x4(planeCenter);
Vector3 normal = new Vector3((float)planeModel.x,
(float)planeModel.y,
(float)planeModel.z);
normal = m_mostRecentUnityWorldTDepthCamera.MultiplyVector(normal);
Vector3.Normalize(normal);
float distance = (float)planeModel.w / normal.magnitude;
plane = new Plane(normal, distance);
return(returnValue);
}
public static void DrawLine(this BitmapSurface surface, int color, DVector4 p1, DVector4 p2)
{
unchecked {
DrawLine(surface, color, p1.X, p1.Y, p2.X, p2.Y);
}
}
public DisplayNumericPropertyAttribute(DVector4 Default, double Increment, int Decimals, string Name, double Minimum = (double)long.MinValue, double Maximum = (double)long.MaxValue)
: this(Default, Name, Increment, Minimum, Maximum, Decimals)
{
}
public void Accept(DVector4 type, TType x, List <ResourceInfo> y)
{
}
public bool Accept(DVector4 type)
{
return(type.Value == Vector4.Zero);
}
public void Accept(DVector4 type, RawTextTable x)
{
}
public override string Accept(DVector4 type)
{
var v = type.Value;
return($"{{x:{v.X},y:{v.Y},z:{v.Z},w:{v.W}}}");
}
private void InclinedCylinder()
{
double step = 1.0 / ApproxLevel;
int k = 2 + (ApproxLevel - 1) * 3;
DVector2 slopeStep = Slope / ApproxLevel;
double s = -0.5;
List <Vertex> v = new List <Vertex>(k * ApproxLevel);
for (int j = 0; j < k; ++j)
{
for (int i = 0; i < ApproxLevel; ++i)
{
double angle = 2.0 * Math.PI / ApproxLevel * i;
double x = Math.Cos(angle);
double y = Math.Sin(angle);
if (j < ApproxLevel - 1)
{
double scale = step * (j + 1);
v.Add(new Vertex(new DVector3(x * scale + slopeStep.X * s, y * scale + slopeStep.Y * s, -0.5)));
}
else if (j >= k - ApproxLevel + 1)
{
double scale = step * (k - j);
v.Add(new Vertex(new DVector3(x * scale + slopeStep.X * s, y * scale + slopeStep.Y * s, 0.5)));
}
else
{
v.Add(new Vertex(new DVector3(x + slopeStep.X * s, y + slopeStep.Y * s, step * (j - ApproxLevel + 1) - 0.5)));
s += 1.0 / ApproxLevel;
}
}
}
List <Polygon> p = new List <Polygon>();
for (int j = 0; j < k - 1; ++j)
{
for (int i = 1; i < ApproxLevel; ++i)
{
p.Add(new Polygon(new List <Vertex>()
{
v[j * ApproxLevel + i - 1], v[j * ApproxLevel + i], v[j * ApproxLevel + i + ApproxLevel], v[j * ApproxLevel + i - 1 + ApproxLevel]
}));
}
p.Add(new Polygon(new List <Vertex>()
{
v[j * ApproxLevel + ApproxLevel - 1], v[j * ApproxLevel + 0], v[j * ApproxLevel + ApproxLevel], v[j * ApproxLevel + ApproxLevel + ApproxLevel - 1]
}));
}
List <Vertex> list = v.GetRange(0, ApproxLevel);
list.Reverse();
p.Add(new Polygon(list));
p.Add(new Polygon(v.GetRange(v.Count - ApproxLevel, ApproxLevel)));
List <uint> ind = new List <uint>();
List <GLVertex> gv = new List <GLVertex>(v.Capacity);
foreach (Vertex vert in v)
{
DVector4 norm = DVector4.Zero;
foreach (Polygon pol in vert.Polygon)
{
norm += pol._Normal;
}
vert._Normal = norm / vert.Polygon.Count;
gv.Add(new GLVertex((float)vert._Point.X, (float)vert._Point.Y, (float)vert._Point.Z,
(float)vert._Normal.X, (float)vert._Normal.Y, (float)vert._Normal.Z,
(float)ObjectColor.X, (float)ObjectColor.Y, (float)ObjectColor.Z));
}
foreach (Polygon pol in p)
{
uint start = (uint)v.FindIndex((Vertex curVert) => curVert == pol.Vertex[0]);
int count = pol.Vertex.Count;
for (int i = 1; i < count - 1; ++i)
{
ind.Add(start);
ind.Add((uint)v.FindIndex((Vertex curVert) => curVert == pol.Vertex[i]));
ind.Add((uint)v.FindIndex((Vertex curVert) => curVert == pol.Vertex[i + 1]));
}
}
GLVertecis = gv.ToArray();
indices = ind.ToArray();
}
private void InclinedCylinder()
{
double step = 1.0 / ApproxLevel;
int k = 2 + (ApproxLevel - 1) * 3;
DVector2 slopeStep = Slope / ApproxLevel;
double s = -0.5;
List <Vertex> v = new List <Vertex>(k * ApproxLevel);
for (int j = 0; j < k; ++j)
{
for (int i = 0; i < ApproxLevel; ++i)
{
double angle = 2.0 * Math.PI / ApproxLevel * i;
double x = Math.Cos(angle);
double y = Math.Sin(angle);
if (j < ApproxLevel - 1)
{
double scale = step * (j + 1);
v.Add(new Vertex(new DVector3(x * scale + slopeStep.X * s, y * scale + slopeStep.Y * s, -0.5)));
}
else if (j >= k - ApproxLevel + 1)
{
double scale = step * (k - j);
v.Add(new Vertex(new DVector3(x * scale + slopeStep.X * s, y * scale + slopeStep.Y * s, 0.5)));
}
else
{
v.Add(new Vertex(new DVector3(x + slopeStep.X * s, y + slopeStep.Y * s, step * (j - ApproxLevel + 1) - 0.5)));
s += 1.0 / ApproxLevel;
}
}
}
List <Polygon> p = new List <Polygon>();
for (int j = 0; j < k - 1; ++j)
{
for (int i = 1; i < ApproxLevel; ++i)
{
p.Add(new Polygon(new List <Vertex>()
{
v[j * ApproxLevel + i - 1], v[j * ApproxLevel + i], v[j * ApproxLevel + i + ApproxLevel], v[j * ApproxLevel + i - 1 + ApproxLevel]
}));
}
p.Add(new Polygon(new List <Vertex>()
{
v[j * ApproxLevel + ApproxLevel - 1], v[j * ApproxLevel + 0], v[j * ApproxLevel + ApproxLevel], v[j * ApproxLevel + ApproxLevel + ApproxLevel - 1]
}));
}
List <Vertex> list = v.GetRange(0, ApproxLevel);
list.Reverse();
p.Add(new Polygon(list));
p.Add(new Polygon(v.GetRange(v.Count - ApproxLevel, ApproxLevel)));
List <uint> ind = new List <uint>();
foreach (Vertex vert in v)
{
DVector4 norm = DVector4.Zero;
foreach (Polygon pol in vert.Polygon)
{
norm += pol._Normal;
}
vert._Normal = norm / vert.Polygon.Count;
}
Vertecis = v.ToArray();
Polygons = p.ToArray();
}
public void Accept(DVector4 type, DefField x, List <ResourceInfo> y)
{
throw new NotImplementedException();
}
public void Accept(DVector4 type, StringBuilder x)
{
var v = type.Value;
x.Append($"vector4(x={v.X},y={v.Y},z={v.Z},w={v.W})");
}
public void Accept(DVector4 type, ByteBuf x)
{
x.WriteVector4(type.Value);
}
